Hey guys! Here is a new tutorial to guide you step by step while making this kind of super amazing electronic projects, which is the "crawler robot" also known as a "spider robot" or a "quadruped robot."
Since everybody noticed the high speed evolution of robotics technology, we decided to take you guys to a higher level on robotics and robot making. we started a while ago by making some basic electronic projects and basic robot like PICTO92 the line follower robot in order to make you a bit familiar with the electronic stuff and find yourself able to invent your own projects.
Moving to another level, we've started with this robot which is a basic one in the concept but it will become a bit complicated if you get deeper in its program. And since these gadgets are so expensive in the webstore we provide this step by step guidance to guide you guys making your own Spiderbot
This project is so handy to make specially after getting the customized PCB that we’ve ordered from JLCPCB to improve the appearance of our robot and also there is enough documents and codes in this guide to allow you create your crawler easily.
We've made this project in just 7 days only, just two days to finish the hardware making and the assemble, then five days to prepare the code and the android app. in order to control the robot through it. Before starting let’s see first
What You Will Learn:
Selecting the right components depending on your project functionalities
Making the circuit to connect all the choosen components
Assemble all the project parts
Scaling of the robot balance
Using the Android app. to connect through Bluetooth and start manipulating the system
Step 1: What Is a "Spider Robot?"
As its name defines it, our robot is a basic representation of the sipder movements but it will not perform exactly the same body moves since we are using only four legs instead of eight legs.
Named also a Quadrupedrobot since it has four legs and make its movements using these legs, the movment of each leg is related to the other legs in order to identify the roboty body postion and also to control the robot body balance.
Legged robots handle terrain better than their wheeled counterparts and move in varied and animalistic ways. However, this makes legged robots more complicated, and less accessible to many makers. and also the making cost and the high depenses that a maker should spend in order to create a full body quadruped since it is based on servo motors or stepper motors and both are more expensive than DC motors that could be used in wheeled robots.
Advantages
You will find quadrupeds abundant in nature, because four legs allow for passive stability, or the ability to stay standing without actively adjusting position. The same is true of robots. A four-legged robot is cheaper and simpler than a robot with more legs, yet it can still achieve stability.
Step 2: Servo Motors Are the Main Actuators
A servomotor as defined in wikipedia, is a rotary actuator or linear actuator that allows for precise control of angular or linear position, velocity and acceleration.[1]It consists of a suitable motor coupled to a sensor for position feedback. It also requires a relatively sophisticated controller, often a dedicated module designed specifically for use with servomotors.
Servomotors are not a specific class of motor although the term servomotor is often used to refer to a motor suitable for use in a closed-loop control system.
Generally speaking the control signal is a square wave pulse train. Common frequencies for control signals are 44Hz, 50Hz, and 400Hz. The positive pulse width is what determines the servo position. A positive pulse width of around 0.5ms will cause the servo horn to deflect as much as it can to the left (generally around 45 to 90 degrees depending upon the servo in question). A positive pulse width of around 2.5ms to 3.0ms will cause the servo to deflect to the right as far as it can. A pulse width of around 1.5ms will cause the servo to hold the neutral position at 0 degrees. The output high voltage is generally something between 2.5 volts and 10 volts (with 3V typical). The output low voltage ranges from -40mV to 0V.
Step 3: The PCB Making (Produced by JLCPCB)
About JLCPCB
JLCPCB (Shenzhen JIALICHUANG Electronic Technology Development Co., Ltd.), is the largest PCB prototype enterprise in China and a high-tech manufacturer specializing in quick PCB prototype and small-batch PCB production.
With over 10 years of experience in PCB manufacturing, JLCPCB has more than 200,000 customers at home and abroad, with over 8,000 online orders of PCB prototyping and small quantity PCB production per day. The annual production capacity is 200,000 sq.m. for various of 1-layer, 2-layer or multi-layer PCBs. JLC is a professional PCB manufacturer featured of large scale, well equipment, strict management and superior quality.
Back to Our Project
In order to produce the PCB, I have compared the price from many PCB producers and I chose JLCPCB the best PCB suppliers and the cheapest PCB providers to order this circuit. All what I need to do is some simple clicks to upload the gerber file and set some parameters like the PCB thickness color and quantity, then I’ve paid just 2 Dollars to get my PCB after five days only.
As it shows the picture of the related schemtic, I have used an Arduino Nano to control the whole system also I've designed the robot spider shape to make this project much more better.
You can get the Circuit (PDF) file from here. As you can see in the pictures above the PCB is very well manufactured and I’ve got the same PCB spider shape that we’ve designed and all the labels and logos are there to guide me during the soldering steps.
You can also download the Gerber file for this circuit from here in the case you want to place an order for the same circuit design.
Step 4: Ingredients
Now let’s review the necessary components that we need for this project, so as I've said, I'm using an Arduino Nano to run all the 12 servo motor of the robot four legs. The project also include an OLED display to display the Cozmo faces and a bluetooth module to control the robot through an android app.
In order to create this kind of projects we will need :
- The PCB that we've order it from JLCPCB
- 12 Servo motors as you remember 3 servos for each leg : https://amzn.to/2B25XbG
- One Arduino Nano : https://amzn.to/2MmZsVg
- HC-06 Bluetooth module : https://amzn.to/2B1Z3CY
- One OLED Display screen : https://amzn.to/2OySnyn
- 5mm RGB LEDs : https://amzn.to/2B56hq3
- Some header connetcors : https://amzn.to/2nyZg7i
- And the robot body peaces that you need to print them using a 3D printer
Step 5: The Robot Assemble
Now we have the PCB ready and all the components soldered very well, after that we need to assemble the robot body, the procedure is so easy so just follow the steps that I show, we need first to prepare each leg a side and to make one led we need two servo motors for the joints and the Coxa, Femur and Tibia printed parts with this small attach part.
About the robot's Body pieces you can download its STL files from here.
Starting with the first servo, place it in its socket and hold it with its screws, after that turn the servos axe to 180° without placing the screw for the attaches and move to the next part wich is the Femur to connect it to the tibia using the first servo joint axe and the attach piece. The last step to complete the leg is placing the second joint I mean the second servo to hold the third part of the leg which is the Coxa piece.
Now repeat the same thing for all legs to get four legs done ready. After that take the upper chassis and place the rest of servos in them sockets and then connect each leg to the appropriate servo. There is only one last printed part which is the bottom robot chassis in where we will place our circuit board
Step 6: The Android App
Talking about the android up it allows you to
connect to your robot through Bluetooth and make forward and backward movements and left right turnings, it allows you also to control the robot light color in real time by picking the desired color from this color wheel.
You can download the android app for free from this link over :here
Step 7: The Arduino Code and Test Validation
Now we have the robot almost ready to run but we need to set up the joints angles first, so upload the setup code which allows you to put each servo in the right position by attaching the servos in 90 degrees do not forget to connect the 7V DC battery in order to run the robot.
Next we need to upload the main program to control the robot using the android app. Both programs you can download them from the these links:
- Scaling servo code : download link- Spider robot main program : download link
After uploading the code I have connected the OLED display in order to display Cozmo robot smiles that I’ve made in the main code.
As you can see guys in the pictures above, the robot follows all the instructions sent from my smartphone and still some other improvements to perform in order to make it much more bett
Spider Robot main code
Arduino
/************************************************************************************************************************************************************************
* - Author : BELKHIR Mohamed *
* - Profession : (Electrical Ingineer) MEGA DAS owner *
* - Main purpose : Industrial Application *
* - Copyright (c) holder : All rights reserved *
* - License : BSD 2-Clause License *
* - Date : 20/04/2017 *
* ***********************************************************************************************************************************************************************/
/*********************************** NOTE **************************************/
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright notice, this
// list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED
/*
*/
#include <Servo.h> //to define and control servos
#include <FlexiTimer2.h>//to set a timer to manage all servos
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
// OLED display TWI address
#define OLED_ADDR 0x3C
Adafruit_SSD1306 display(-1);
/* Servos --------------------------------------------------------------------*/
//define 12 servos for 4 legs
Servo servo[4][3];
//define servos' ports
const int servo_pin[4][3] = { {11, 12, 13}, {2, 4, 7}, {14, 15, 16},{8, 9, 10} };/* Size of the robot ---------------------------------------------------------*/
const float length_a = 50;
const float length_b = 77.1;
const float length_c = 27.5;
const float length_side = 71;
const float z_absolute = -28;
/* Constants for movement ----------------------------------------------------*/
const float z_default = -50, z_up = -30, z_boot = z_absolute;
const float x_default = 62, x_offset = 0;
const float y_start = 0, y_step = 40;
const float y_default = x_default;
/* variables for movement ----------------------------------------------------*/
volatile float site_now[4][3]; //real-time coordinates of the end of each leg
volatile float site_expect[4][3]; //expected coordinates of the end of each leg
float temp_speed[4][3]; //each axis' speed, needs to be recalculated before each movement
float move_speed; //movement speed
float speed_multiple = 1; //movement speed multiple
const float spot_turn_speed = 4;
const float leg_move_speed = 8;
const float body_move_speed = 3;
const float stand_seat_speed = 1;
volatile int rest_counter; //+1/0.02s, for automatic rest
//functions' parameter
const float KEEP = 255;
//define PI for calculation
const float pi = 3.1415926;
/* Constants for turn --------------------------------------------------------*/
//temp length
const float temp_a = sqrt(pow(2 * x_default + length_side, 2) + pow(y_step, 2));
const float temp_b = 2 * (y_start + y_step) + length_side;
const float temp_c = sqrt(pow(2 * x_default + length_side, 2) + pow(2 * y_start + y_step + length_side, 2));
const float temp_alpha = acos((pow(temp_a, 2) + pow(temp_b, 2) - pow(temp_c, 2)) / 2 / temp_a / temp_b);
//site for turn
const float turn_x1 = (temp_a - length_side) / 2;
const float turn_y1 = y_start + y_step / 2;
const float turn_x0 = turn_x1 - temp_b * cos(temp_alpha);
const float turn_y0 = temp_b * sin(temp_alpha) - turn_y1 - length_side;
const int lightR=3;
const int lightG=5;
const int lightB=6;
int LedR=0;
int LedG=0;
int LedB=0;
char SerialData; // Use this variable to read each caractere received through serial port
String data="";
void setup()
{
Serial.begin(9600);
display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
display.clearDisplay();
display.display();
delay(10000);
set_site(0, x_default - x_offset, y_start + y_step, z_boot);
set_site(1, x_default - x_offset, y_start + y_step, z_boot);
set_site(2, x_default + x_offset, y_start, z_boot);
set_site(3, x_default + x_offset, y_start, z_boot);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 3; j++)
{
site_now[i][j] = site_expect[i][j];
}
}
//start servo service
FlexiTimer2::set(20, servo_service);
FlexiTimer2::start();
//initialize servos
servo_attach();
stand();
// delay(3000);
// sit();
// delay(3000);
// stand();
// delay(3000);
happy();
delay (random (500, 1000));
cierra();
delay (150);
enfado();
delay (random (1000, 3000));
cierra();
delay (150);
entorna();
delay (random (1000, 3000));
cierra();
delay (150);
enfado1();
delay (random (1000, 3000));
cierra();
delay (150);
triste();
delay (random (1000, 3000));
cierra();
delay (150);
abre();
delay (random (500, 3000));
cierra();
delay (150);
happy();
delay (random (500, 1000));
}
void loop()
{
while(Serial.available()) // While serial data are available we store it
{
delay(10);
SerialData=Serial.read();
if(SerialData=='b')
LedR=Serial.parseInt();
else if(SerialData=='g')
LedG=Serial.parseInt();
else if(SerialData=='r')
LedB=Serial.parseInt();
else
data+=SerialData;
}
if(data=="f") // If the stored data is forward movement
{
cierra();
delay (150);
happy();
step_forward(1);
}
if(data=="p") // If the stored data is backward movement
{
cierra();
delay (150);
triste();
step_back(1);
}
if(data=="l") // If the stored data is to turn left the car
{
cierra();
delay (150);
enfado1();
turn_left(1);
}
if(data=="m") // If the stored data is to turn right the car
{
cierra();
delay (150);
enfado();
turn_right(5);
}
data="";
analogWrite(lightR,LedR);
analogWrite(lightG,LedG);
analogWrite(lightB,LedB);
}
void servo_attach(void)
{
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 3; j++)
{
servo[i][j].attach(servo_pin[i][j]);
delay(100);
}
}
}
void servo_detach(void)
{
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 3; j++)
{
servo[i][j].detach();
delay(100);
}
}
}
void sit(void)
{
move_speed = stand_seat_speed;
for (int leg = 0; leg < 4; leg++)
{
set_site(leg, KEEP, KEEP, z_boot);
}
wait_all_reach();
}
/*
- stand
- blocking function
---------------------------------------------------------------------------*/
void stand(void)
{
move_speed = stand_seat_speed;
for (int leg = 0; leg < 4; leg++)
{
set_site(leg, KEEP, KEEP, z_default);
}
wait_all_reach();
}
/*
- spot turn to left
- blocking function
- parameter step steps wanted to turn
---------------------------------------------------------------------------*/
void turn_left(unsigned int step)
{
move_speed = spot_turn_speed;
while (step-- > 0)
{
if (site_now[3][1] == y_start)
{
//leg 3&1 move
set_site(3, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(0, turn_x1 - x_offset, turn_y1, z_default);
set_site(1, turn_x0 - x_offset, turn_y0, z_default);
set_site(2, turn_x1 + x_offset, turn_y1, z_default);
set_site(3, turn_x0 + x_offset, turn_y0, z_up);
wait_all_reach();
set_site(3, turn_x0 + x_offset, turn_y0, z_default);
wait_all_reach();
set_site(0, turn_x1 + x_offset, turn_y1, z_default);
set_site(1, turn_x0 + x_offset, turn_y0, z_default);
set_site(2, turn_x1 - x_offset, turn_y1, z_default);
set_site(3, turn_x0 - x_offset, turn_y0, z_default);
wait_all_reach();
set_site(1, turn_x0 + x_offset, turn_y0, z_up);
wait_all_reach();
set_site(0, x_default + x_offset, y_start, z_default);
set_site(1, x_default + x_offset, y_start, z_up);
set_site(2, x_default - x_offset, y_start + y_step, z_default);
set_site(3, x_default - x_offset, y_start + y_step, z_default);
wait_all_reach();
set_site(1, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
else
{
//leg 0&2 move
set_site(0, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(0, turn_x0 + x_offset, turn_y0, z_up);
set_site(1, turn_x1 + x_offset, turn_y1, z_default);
set_site(2, turn_x0 - x_offset, turn_y0, z_default);
set_site(3, turn_x1 - x_offset, turn_y1, z_default);
wait_all_reach();
set_site(0, turn_x0 + x_offset, turn_y0, z_default);
wait_all_reach();
set_site(0, turn_x0 - x_offset, turn_y0, z_default);
set_site(1, turn_x1 - x_offset, turn_y1, z_default);
set_site(2, turn_x0 + x_offset, turn_y0, z_default);
set_site(3, turn_x1 + x_offset, turn_y1, z_default);
wait_all_reach();
set_site(2, turn_x0 + x_offset, turn_y0, z_up);
wait_all_reach();
set_site(0, x_default - x_offset, y_start + y_step, z_default);
set_site(1, x_default - x_offset, y_start + y_step, z_default);
set_site(2, x_default + x_offset, y_start, z_up);
set_site(3, x_default + x_offset, y_start, z_default);
wait_all_reach();
set_site(2, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
}
}
/*
- spot turn to right
- blocking function
- parameter step steps wanted to turn
---------------------------------------------------------------------------*/
void turn_right(unsigned int step)
{
move_speed = spot_turn_speed;
while (step-- > 0)
{
if (site_now[2][1] == y_start)
{
//leg 2&0 move
set_site(2, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(0, turn_x0 - x_offset, turn_y0, z_default);
set_site(1, turn_x1 - x_offset, turn_y1, z_default);
set_site(2, turn_x0 + x_offset, turn_y0, z_up);
set_site(3, turn_x1 + x_offset, turn_y1, z_default);
wait_all_reach();
set_site(2, turn_x0 + x_offset, turn_y0, z_default);
wait_all_reach();
set_site(0, turn_x0 + x_offset, turn_y0, z_default);
set_site(1, turn_x1 + x_offset, turn_y1, z_default);
set_site(2, turn_x0 - x_offset, turn_y0, z_default);
set_site(3, turn_x1 - x_offset, turn_y1, z_default);
wait_all_reach();
set_site(0, turn_x0 + x_offset, turn_y0, z_up);
wait_all_reach();
set_site(0, x_default + x_offset, y_start, z_up);
set_site(1, x_default + x_offset, y_start, z_default);
set_site(2, x_default - x_offset, y_start + y_step, z_default);
set_site(3, x_default - x_offset, y_start + y_step, z_default);
wait_all_reach();
set_site(0, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
else
{
//leg 1&3 move
set_site(1, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(0, turn_x1 + x_offset, turn_y1, z_default);
set_site(1, turn_x0 + x_offset, turn_y0, z_up);
set_site(2, turn_x1 - x_offset, turn_y1, z_default);
set_site(3, turn_x0 - x_offset, turn_y0, z_default);
wait_all_reach();
set_site(1, turn_x0 + x_offset, turn_y0, z_default);
wait_all_reach();
set_site(0, turn_x1 - x_offset, turn_y1, z_default);
set_site(1, turn_x0 - x_offset, turn_y0, z_default);
set_site(2, turn_x1 + x_offset, turn_y1, z_default);
set_site(3, turn_x0 + x_offset, turn_y0, z_default);
wait_all_reach();
set_site(3, turn_x0 + x_offset, turn_y0, z_up);
wait_all_reach();
set_site(0, x_default - x_offset, y_start + y_step, z_default);
set_site(1, x_default - x_offset, y_start + y_step, z_default);
set_site(2, x_default + x_offset, y_start, z_default);
set_site(3, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(3, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
}
}
/*
- go forward
- blocking function
- parameter step steps wanted to go
---------------------------------------------------------------------------*/
void step_forward(unsigned int step)
{
move_speed = leg_move_speed;
while (step-- > 0)
{
if (site_now[2][1] == y_start)
{
//leg 2&1 move
set_site(2, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default);
wait_all_reach();
move_speed = body_move_speed;
set_site(0, x_default + x_offset, y_start, z_default);
set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default);
set_site(2, x_default - x_offset, y_start + y_step, z_default);
set_site(3, x_default - x_offset, y_start + y_step, z_default);
wait_all_reach();
move_speed = leg_move_speed;
set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(1, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(1, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
else
{
//leg 0&3 move
set_site(0, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default);
wait_all_reach();
move_speed = body_move_speed;
set_site(0, x_default - x_offset, y_start + y_step, z_default);
set_site(1, x_default - x_offset, y_start + y_step, z_default);
set_site(2, x_default + x_offset, y_start, z_default);
set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default);
wait_all_reach();
move_speed = leg_move_speed;
set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(3, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(3, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
}
}
/*
- go back
- blocking function
- parameter step steps wanted to go
---------------------------------------------------------------------------*/
void step_back(unsigned int step)
{
move_speed = leg_move_speed;
while (step-- > 0)
{
if (site_now[3][1] == y_start)
{
//leg 3&0 move
set_site(3, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default);
wait_all_reach();
move_speed = body_move_speed;
set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default);
set_site(1, x_default + x_offset, y_start, z_default);
set_site(2, x_default - x_offset, y_start + y_step, z_default);
set_site(3, x_default - x_offset, y_start + y_step, z_default);
wait_all_reach();
move_speed = leg_move_speed;
set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(0, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(0, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
else
{
//leg 1&2 move
set_site(1, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default);
wait_all_reach();
move_speed = body_move_speed;
set_site(0, x_default - x_offset, y_start + y_step, z_default);
set_site(1, x_default - x_offset, y_start + y_step, z_default);
set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default);
set_site(3, x_default + x_offset, y_start, z_default);
wait_all_reach();
move_speed = leg_move_speed;
set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(2, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(2, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
}
}
// add by RegisHsu
void body_left(int i)
{
set_site(0, site_now[0][0] + i, KEEP, KEEP);
set_site(1, site_now[1][0] + i, KEEP, KEEP);
set_site(2, site_now[2][0] - i, KEEP, KEEP);
set_site(3, site_now[3][0] - i, KEEP, KEEP);
wait_all_reach();
}
void body_right(int i)
{
set_site(0, site_now[0][0] - i, KEEP, KEEP);
set_site(1, site_now[1][0] - i, KEEP, KEEP);
set_site(2, site_now[2][0] + i, KEEP, KEEP);
set_site(3, site_now[3][0] + i, KEEP, KEEP);
wait_all_reach();
}
void hand_wave(int i)
{
float x_tmp;
float y_tmp;
float z_tmp;
move_speed = 1;
if (site_now[3][1] == y_start)
{
body_right(15);
x_tmp = site_now[2][0];
y_tmp = site_now[2][1];
z_tmp = site_now[2][2];
move_speed = body_move_speed;
for (int j = 0; j < i; j++)
{
set_site(2, turn_x1, turn_y1, 50);
wait_all_reach();
set_site(2, turn_x0, turn_y0, 50);
wait_all_reach();
}
set_site(2, x_tmp, y_tmp, z_tmp);
wait_all_reach();
move_speed = 1;
body_left(15);
}
else
{
body_left(15);
x_tmp = site_now[0][0];
y_tmp = site_now[0][1];
z_tmp = site_now[0][2];
move_speed = body_move_speed;
for (int j = 0; j < i; j++)
{
set_site(0, turn_x1, turn_y1, 50);
wait_all_reach();
set_site(0, turn_x0, turn_y0, 50);
wait_all_reach();
}
set_site(0, x_tmp, y_tmp, z_tmp);
wait_all_reach();
move_speed = 1;
body_right(15);
}
}
void hand_shake(int i)
{
float x_tmp;
float y_tmp;
float z_tmp;
move_speed = 1;
if (site_now[3][1] == y_start)
{
body_right(15);
x_tmp = site_now[2][0];
y_tmp = site_now[2][1];
z_tmp = site_now[2][2];
move_speed = body_move_speed;
for (int j = 0; j < i; j++)
{
set_site(2, x_default - 30, y_start + 2 * y_step, 55);
wait_all_reach();
set_site(2, x_default - 30, y_start + 2 * y_step, 10);
wait_all_reach();
}
set_site(2, x_tmp, y_tmp, z_tmp);
wait_all_reach();
move_speed = 1;
body_left(15);
}
else
{
body_left(15);
x_tmp = site_now[0][0];
y_tmp = site_now[0][1];
z_tmp = site_now[0][2];
move_speed = body_move_speed;
for (int j = 0; j < i; j++)
{
set_site(0, x_default - 30, y_start + 2 * y_step, 55);
wait_all_reach();
set_site(0, x_default - 30, y_start + 2 * y_step, 10);
wait_all_reach();
}
set_site(0, x_tmp, y_tmp, z_tmp);
wait_all_reach();
move_speed = 1;
body_right(15);
}
}
void head_up(int i)
{
set_site(0, KEEP, KEEP, site_now[0][2] - i);
set_site(1, KEEP, KEEP, site_now[1][2] + i);
set_site(2, KEEP, KEEP, site_now[2][2] - i);
set_site(3, KEEP, KEEP, site_now[3][2] + i);
wait_all_reach();
}
void head_down(int i)
{
set_site(0, KEEP, KEEP, site_now[0][2] + i);
set_site(1, KEEP, KEEP, site_now[1][2] - i);
set_site(2, KEEP, KEEP, site_now[2][2] + i);
set_site(3, KEEP, KEEP, site_now[3][2] - i);
wait_all_reach();
}
//void body_dance(int i)
//{
// float x_tmp;
// float y_tmp;
// float z_tmp;
// float body_dance_speed = 2;
// sit();
// move_speed = 1;
// set_site(0, x_default, y_default, KEEP);
// set_site(1, x_default, y_default, KEEP);
// set_site(2, x_default, y_default, KEEP);
// set_site(3, x_default, y_default, KEEP);
// wait_all_reach();
// //stand();
// set_site(0, x_default, y_default, z_default - 20);
// set_site(1, x_default, y_default, z_default - 20);
// set_site(2, x_default, y_default, z_default - 20);
// set_site(3, x_default, y_default, z_default - 20);
// wait_all_reach();
// move_speed = body_dance_speed;
// head_up(30);
// for (int j = 0; j < i; j++)
// {
// if (j > i / 4)
// move_speed = body_dance_speed * 2;
// if (j > i / 2)
// move_speed = body_dance_speed * 3;
// set_site(0, KEEP, y_default - 20, KEEP);
// set_site(1, KEEP, y_default + 20, KEEP);
// set_site(2, KEEP, y_default - 20, KEEP);
// set_site(3, KEEP, y_default + 20, KEEP);
// wait_all_reach();
// set_site(0, KEEP, y_default + 20, KEEP);
// set_site(1, KEEP, y_default - 20, KEEP);
// set_site(2, KEEP, y_default + 20, KEEP);
// set_site(3, KEEP, y_default - 20, KEEP);
// wait_all_reach();
// }
// move_speed = body_dance_speed;
// head_down(30);
//}
/*
- microservos service /timer interrupt function/50Hz
- when set site expected,this function move the end point to it in a straight line
- temp_speed[4][3] should be set before set expect site,it make sure the end point
move in a straight line,and decide move speed.
---------------------------------------------------------------------------*/
void servo_service(void)
{
sei();
static float alpha, beta, gamma;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 3; j++)
{
if (abs(site_now[i][j] - site_expect[i][j]) >= abs(temp_speed[i][j]))
site_now[i][j] += temp_speed[i][j];
else
site_now[i][j] = site_expect[i][j];
}
cartesian_to_polar(alpha, beta, gamma, site_now[i][0], site_now[i][1], site_now[i][2]);
polar_to_servo(i, alpha, beta, gamma);
}
rest_counter++;
}
/*
- set one of end points' expect site
- this founction will set temp_speed[4][3] at same time
- non - blocking function
---------------------------------------------------------------------------*/
void set_site(int leg, float x, float y, float z)
{
float length_x = 0, length_y = 0, length_z = 0;
if (x != KEEP)
length_x = x - site_now[leg][0];
if (y != KEEP)
length_y = y - site_now[leg][1];
if (z != KEEP)
length_z = z - site_now[leg][2];
float length = sqrt(pow(length_x, 2) + pow(length_y, 2) + pow(length_z, 2));
temp_speed[leg][0] = length_x / length * move_speed * speed_multiple;
temp_speed[leg][1] = length_y / length * move_speed * speed_multiple;
temp_speed[leg][2] = length_z / length * move_speed * speed_multiple;
if (x != KEEP)
site_expect[leg][0] = x;
if (y != KEEP)
site_expect[leg][1] = y;
if (z != KEEP)
site_expect[leg][2] = z;
}
/*
- wait one of end points move to expect site
- blocking function
---------------------------------------------------------------------------*/
void wait_reach(int leg)
{
while (1)
if (site_now[leg][0] == site_expect[leg][0])
if (site_now[leg][1] == site_expect[leg][1])
if (site_now[leg][2] == site_expect[leg][2])
break;
}
/*
- wait all of end points move to expect site
- blocking function
---------------------------------------------------------------------------*/
void wait_all_reach(void)
{
for (int i = 0; i < 4; i++)
wait_reach(i);
}
/*
- trans site from cartesian to polar
- mathematical model 2/2
---------------------------------------------------------------------------*/
void cartesian_to_polar(volatile float &alpha, volatile float &beta, volatile float &gamma, volatile float x, volatile float y, volatile float z)
{
//calculate w-z degree
float v, w;
w = (x >= 0 ? 1 : -1) * (sqrt(pow(x, 2) + pow(y, 2)));
v = w - length_c;
alpha = atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2) + pow(z, 2)) / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2)));
beta = acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) / 2 / length_a / length_b);
//calculate x-y-z degree
gamma = (w >= 0) ? atan2(y, x) : atan2(-y, -x);
//trans degree pi->180
alpha = alpha / pi * 180;
beta = beta / pi * 180;
gamma = gamma / pi * 180;
}
/*
- trans site from polar to microservos
- mathematical model map to fact
- the errors saved in eeprom will be add
---------------------------------------------------------------------------*/
void polar_to_servo(int leg, float alpha, float beta, float gamma)
{
if (leg == 0)
{
alpha = 90 - alpha;
beta = beta;
gamma += 90;
}
else if (leg == 1)
{
alpha += 90;
beta = 180 - beta;
gamma = 90 - gamma;
}
else if (leg == 2)
{
alpha += 90;
beta = 180 - beta;
gamma = 90 - gamma;
}
else if (leg == 3)
{
alpha = 90 - alpha;
beta = beta;
gamma += 90;
}
servo[leg][0].write(alpha);
servo[leg][1].write(beta);
servo[leg][2].write(gamma);
}
void abre() {
display.clearDisplay();
display.fillCircle (50, 15, 12, WHITE); //ojo izquierdo grande
display.fillCircle (82, 20, 7, WHITE); //ojo derecho pequeo
display.display();
}
void cierra() {
display.clearDisplay();
display.drawFastHLine(40, 15, 20, WHITE);
display.drawFastHLine(72, 20, 15, WHITE);
display.display();
}
void entorna() {
display.clearDisplay();
display.fillCircle (42, 10, 20, WHITE); //ojo izquierdo grande
display.fillCircle (82, 10, 15, WHITE); //ojo derecho pequeo
display.fillRect (0, 0, 128, 15, BLACK); //ceja superior
display.fillRect (0, 40, 128, 15, BLACK); //ceja inferior
display.display();
}
void triste() {
display.clearDisplay();
display.fillCircle (42, 10, 17, WHITE); //ojo izquierdo grande
display.fillCircle (82, 10, 17, WHITE); //ojo derecho pequeo
display.fillTriangle (0, 0, 0, 35, 78, 0, BLACK); //ceja superior
display.fillTriangle (50, 0, 128, 35, 128, 0, BLACK); //ceja superior
display.display();
}
void happy() {
display.clearDisplay();
display.fillCircle (42, 25, 15, WHITE); //ojo izquierdo grande
display.fillCircle (82, 25, 15, WHITE); //ojo derecho pequeo
display.fillCircle (42, 33, 20, BLACK); //ojo izquierdo grande
display.fillCircle (82, 33, 20, BLACK); //ojo derecho pequeo
display.display();
}
void enfado() {
display.clearDisplay();
display.fillCircle (42, 10, 18, WHITE); //ojo izquierdo grande
display.fillCircle (82, 10, 12, WHITE); //ojo derecho pequeo
display.fillTriangle (0, 0, 54, 26, 118, 0, BLACK); //ceja superior
display.display();
}
void enfado1() {
display.clearDisplay();
display.fillCircle (42, 10, 18, WHITE); //ojo izquierdo grande
display.fillCircle (82, 10, 12, WHITE); //ojo derecho pequeo
display.fillTriangle (0, 0, 65, 15, 120, 0, BLACK); //ceja superior
display.display();
}
Comentarios