/* ----------------------------------------------------------------------------- | |
- Project: Remote control Crawling robot | |
- Author: panerqiang@sunfounder.com, modified by Shane Celis @shanecelis | |
- License: GPL v3 | |
- Date: 2015/2/10 | |
* ----------------------------------------------------------------------------- | |
- Overview | |
- This project was written for the Crawling robot desigened by Sunfounder. | |
This version of the robot has 4 legs, and each leg is driven by 3 servos. | |
This robot is driven by a Ardunio Nano Board with an expansion Board. | |
We recommend that you view the product documentation before using. | |
- Request | |
- This project requires some library files, which you can find in the head of | |
this file. Make sure you have installed these files. | |
- How to | |
- Before use,you must to adjust the robot,in order to make it more accurate. | |
- Adjustment operation | |
1.uncomment ADJUST, make and run | |
2.comment ADJUST, uncomment VERIFY | |
3.measure real sites and set to real_site[4][3], make and run | |
4.comment VERIFY, make and run | |
The document describes in detail how to operate. | |
* ---------------------------------------------------------------------------*/ | |
/* Includes ------------------------------------------------------------------*/ | |
#include <Servo.h> //to define and control servos | |
#include <FlexiTimer2.h>//to set a timer to manage all servos | |
#include <EEPROM.h> //to save errors of all servos | |
/* #include <SPI.h> //nRF24L01 module need 1/3 */ | |
/* #include <nRF24L01.h> //nRF24L01 module need 2/3 */ | |
/* #include <RF24.h> //nRF24L01 module need 3/3 */ | |
/* Installation and Adjustment -----------------------------------------------*/ | |
//#define INSTALL //uncomment only this to install the robot | |
//#define ADJUST //uncomment only this to adjust the servos | |
//#define VERIFY //uncomment only this to verify the adjustment | |
const float adjust_site[3] = { 100, 80, 42 }; | |
/*const float real_site[4][3] = { { 100, 80, 42 }, { 100, 80, 42 }, | |
{ 100, 80, 42 }, { 100, 80, 42 } };*/ | |
const float real_site[4][3] = { { 98, 80, 28 }, { 83, 89, 24 }, | |
{ 91, 72, 37 }, { 63, 99, 40 } }; | |
/* Servos --------------------------------------------------------------------*/ | |
//define 12 servos for 4 legs | |
Servo servo[4][3]; | |
//define servos' ports | |
const int servo_pin[4][3] = { 2, 3, 4, 5, 6, 7, 14, 15, 16, 17, 18, 19 }; | |
/* Wireless communication ----------------------------------------------------*/ | |
//dfine RF24 for nRF24l01 | |
//RF24 radio(9, 10); | |
//define RF24 transmit pipe | |
//NOTE: the "LL" at the end of the constant is "LongLong" type | |
const uint64_t pipe = 0xE8E8F0F0E1LL; | |
/* Size of the robot ---------------------------------------------------------*/ | |
const float length_a = 55; | |
const float length_b = 80; | |
const float length_c = 22.75; | |
const float length_side = 66; | |
const float z_absolute = -12; | |
/* Constants for movement ----------------------------------------------------*/ | |
const float z_default = -25, z_boot = z_absolute; | |
const float x_default = 55; | |
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 | |
volatile float site_now_polar[4][3]; //real-time degrees of each leg segment | |
volatile float site_expect_polar[4][3]; //expected degrees end of each leg segment | |
float temp_speed[4][3]; //each axis' speed, needs to be recalculated before each movement | |
float move_speed = 1.4; //movement speed | |
float move_speed_polar = 20.0f/50.0f; // degree/sec/50hz | |
float speed_multiple = 1; //movement speed multiple | |
//functions' parameter | |
const float KEEP = 255; | |
const float stand_seat_speed = 1; | |
const unsigned long wait_to_sleep = 3000; // milliseconds | |
const bool use_polar = true; | |
const bool use_bytes = true; | |
unsigned long last_commanded_at = 0; // millis() | |
/* ---------------------------------------------------------------------------*/ | |
/* | |
- setup function | |
* ---------------------------------------------------------------------------*/ | |
void setup() | |
{ | |
#ifdef INSTALL | |
//initialize all servos | |
for (int i = 0; i < 4; i++) | |
{ | |
for (int j = 0; j < 3; j++) | |
{ | |
servo[i][j].attach(servo_pin[i][j]); | |
delay(100); | |
} | |
} | |
while (1); | |
#endif | |
#ifdef ADJUST | |
adjust(); | |
while (1); | |
#endif | |
#ifdef VERIFY | |
verify(); | |
while (1); | |
#endif | |
//start serial for debug | |
Serial.begin(115200); | |
Serial.println("Robot starts initialization"); | |
//start listen radio | |
/* radio.begin(); */ | |
/* radio.openReadingPipe(1,pipe); */ | |
/* radio.setRetries(0, 15); */ | |
/* radio.setPALevel(RF24_PA_HIGH); */ | |
/* radio.startListening(); */ | |
/* Serial.println("Radio listening started"); */ | |
//initialize default parameter | |
set_site(0, x_default, y_default, z_boot); | |
set_site(1, x_default, y_default, z_boot); | |
set_site(2, x_default, y_default, z_boot); | |
set_site(3, x_default, y_default, z_boot); | |
for (int i = 0; i < 4; i++) | |
{ | |
for (int j = 0; j < 3; j++) | |
{ | |
site_now[i][j] = site_expect[i][j]; | |
site_now_polar[i][j] = site_expect_polar[i][j] = (j == 0 ? 80.0f : 45.0f); | |
} | |
} | |
//start servo service | |
if (use_polar) | |
FlexiTimer2::set(20, servo_service_polar); | |
else | |
FlexiTimer2::set(20, servo_service); | |
FlexiTimer2::start(); | |
Serial.println("Servo service started"); | |
//initialize servos | |
attach(); | |
Serial.println("Servos initialized"); | |
Serial.println("Robot initialization Complete"); | |
} | |
/* | |
- loop function | |
* ---------------------------------------------------------------------------*/ | |
void loop() | |
{ | |
#ifdef INSTALL | |
while (1); | |
#endif | |
#ifdef ADJUST | |
while (1); | |
#endif | |
#ifdef VERIFY | |
while (1); | |
#endif | |
//put your code here --------------------------------------------------------- | |
Serial.println("Waiting for input..."); | |
while (1) { | |
unsigned long now = millis(); | |
if (Serial.available() > 0) { | |
if (use_bytes) { | |
/* | |
Encode the entire joint state as a stream of 13 bytes. | |
First byte 0 identifies the beginning. | |
*/ | |
if (Serial.available() >= 13) { | |
int byte = Serial.read(); | |
if (byte == 0) { | |
// We've got a begin block. | |
for (int i = 0; i < 12; i++) { | |
byte = Serial.read(); | |
site_expect_polar[i / 3][i % 3] = byte; | |
//Serial.println("set motor " + String(i) + " to " + String(byte)); | |
} | |
} | |
} | |
continue; | |
} | |
String wholeInput = Serial.readString(); | |
int start = 0; | |
int indexOfNewline; | |
do { | |
indexOfNewline = wholeInput.indexOf("\n", start); | |
String input; | |
if (indexOfNewline == -1) { | |
input = wholeInput.substring(start); | |
} else { | |
input = wholeInput.substring(start, indexOfNewline + 1); | |
start = indexOfNewline + 1; | |
} | |
input.trim(); | |
if (input.startsWith("motor ")) { | |
attach_if_needed(); | |
int motor = input.substring(6).toInt(); | |
float value = input.substring(8).toFloat(); | |
site_expect_polar[motor / 3][motor % 3] = value; | |
Serial.println("set motor " + String(motor) + " to " + String(value)); | |
} else if (input.startsWith("tilt") | |
|| input.startsWith("Casso")) { | |
attach_if_needed(); | |
#define rotate_up 20 | |
#define rotate_degree 20 | |
dance_tilt(); | |
} else if (input.startsWith("center") | |
|| input.startsWith("Tripidium")) { | |
attach_if_needed(); | |
dance_center(); | |
sit(); | |
} else if (input.startsWith("nod") | |
|| input.startsWith("Nuo")) { | |
attach_if_needed(); | |
dance_nod(); | |
} else if (input.startsWith("detach")) { | |
attach_if_needed(); | |
detach(); | |
} else if (input.startsWith("round") | |
|| input.startsWith("Vacillo")) { | |
attach_if_needed(); | |
dance_round(); | |
} | |
last_commanded_at = now; | |
} while (indexOfNewline != -1); | |
} | |
if ((now - last_commanded_at) > wait_to_sleep) { | |
//detach(); | |
} | |
} | |
// move_body_absolute(0, 0, z_default - z_boot); | |
// | |
/* Serial.println("Waiting for radio signal"); */ | |
/* byte order,old_order; */ | |
/* bool mode = true; */ | |
/* while (1) */ | |
/* { */ | |
/* if (radio.available()) */ | |
/* { */ | |
/* if (radio.read(&order, 1)) */ | |
/* { */ | |
/* if (order != old_order) */ | |
/* { */ | |
/* Serial.print("Order:"); */ | |
/* Serial.print(order); */ | |
/* Serial.print(" //"); */ | |
/* if (order == 5) */ | |
/* { */ | |
/* if (mode) */ | |
/* { */ | |
/* Serial.println("Rotate Mode"); */ | |
/* move_body_absolute(0, 0, rotate_up); */ | |
/* mode = false; */ | |
/* } */ | |
/* else */ | |
/* { */ | |
/* Serial.println("Move Mode"); */ | |
/* move_body_absolute(0, 0, 0); */ | |
/* mode = true; */ | |
/* } */ | |
/* } */ | |
/* else if (mode) */ | |
/* { */ | |
/* switch (order) */ | |
/* { */ | |
/* case 1: */ | |
/* Serial.println("Move forward"); */ | |
/* move_body_absolute(0, round_length, 0); */ | |
/* break; */ | |
/* case 2: */ | |
/* Serial.println("Move back"); */ | |
/* move_body_absolute(0, -round_length, 0); */ | |
/* break; */ | |
/* case 3: */ | |
/* Serial.println("Move left"); */ | |
/* move_body_absolute(-round_length, 0, 0); */ | |
/* break; */ | |
/* case 4: */ | |
/* Serial.println("Move right"); */ | |
/* move_body_absolute(round_length, 0, 0); */ | |
/* break; */ | |
/* case 0: */ | |
/* Serial.println("Move to origin"); */ | |
/* move_body_absolute(0, 0, 0); */ | |
/* break; */ | |
/* } */ | |
/* } */ | |
/* else */ | |
/* { */ | |
/* switch (order) */ | |
/* { */ | |
/* case 1: */ | |
/* Serial.println("Rotate forward"); */ | |
/* rotate_body_absolute_x(-rotate_degree, rotate_up); */ | |
/* break; */ | |
/* case 2: */ | |
/* Serial.println("Rotate back"); */ | |
/* rotate_body_absolute_x(rotate_degree, rotate_up); */ | |
/* break; */ | |
/* case 3: */ | |
/* Serial.println("Rotate left"); */ | |
/* rotate_body_absolute_y(-rotate_degree, rotate_up); */ | |
/* break; */ | |
/* case 4: */ | |
/* Serial.println("Rotate right"); */ | |
/* rotate_body_absolute_y(rotate_degree, rotate_up); */ | |
/* break; */ | |
/* case 0: */ | |
/* Serial.println("Rotate to origin"); */ | |
/* move_body_absolute(0, 0, rotate_up); */ | |
/* break; */ | |
/* } */ | |
/* } */ | |
/* } */ | |
/* old_order = order; */ | |
/* } */ | |
/* while (radio.read(&order, 1)); */ | |
/* } */ | |
/* } */ | |
//end of your code ----------------------------------------------------------- | |
//while (1); | |
} | |
void detach() { | |
for (int i = 0; i < 4; i++) { | |
for (int j = 0; j < 3; j++) { | |
servo[i][j].detach(); | |
delay(100); | |
} | |
} | |
} | |
void attach() { | |
//initialize servos | |
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 attach_if_needed() { | |
//initialize servos | |
for (int i = 0; i < 4; i++) { | |
for (int j = 0; j < 3; j++) { | |
if (! servo[i][j].attached()) { | |
servo[i][j].attach(servo_pin[i][j]); | |
delay(100); | |
} | |
} | |
} | |
} | |
void dance_center(void) { | |
//#define move_length 40 | |
//#define move_up 40 | |
#define move_length 15 | |
#define move_up 15 | |
move_body_absolute(0, move_length, 0); | |
delay(200); | |
move_body_absolute(-move_length, 0, move_up); | |
move_body_absolute(0, -move_length, 0); | |
move_body_absolute(move_length, 0, move_up); | |
move_body_absolute(0, move_length, 0); | |
move_body_absolute(move_length, 0, move_up); | |
move_body_absolute(0, -move_length, 0); | |
move_body_absolute(-move_length, 0, move_up); | |
move_body_absolute(0, move_length, 0); | |
move_body_absolute(0, 0, 0); | |
delay(200); | |
} | |
void dance_nod() { | |
move_body_absolute(0, 0, rotate_up); | |
rotate_body_absolute_x(-rotate_degree, rotate_up); | |
delay(200); | |
rotate_body_absolute_x(0, rotate_up); | |
rotate_body_absolute_x(-rotate_degree, rotate_up); | |
delay(200); | |
rotate_body_absolute_x(0, rotate_up); | |
move_body_absolute(0, 0, 0); | |
delay(200); | |
} | |
void dance_tilt(void) { | |
move_body_absolute(0, 0, rotate_up); | |
delay(200); | |
rotate_body_absolute_x(-rotate_degree, rotate_up); | |
rotate_body_absolute_x(rotate_degree, rotate_up); | |
move_body_absolute(0, 0, rotate_up); | |
rotate_body_absolute_y(-rotate_degree, rotate_up); | |
rotate_body_absolute_y(rotate_degree, rotate_up); | |
move_body_absolute(0, 0, rotate_up); | |
move_body_absolute(0, 0, 0); | |
delay(200); | |
} | |
void dance_round(void) { | |
//#define round_length 40 | |
#define round_length 20 | |
#define round_up 20 | |
#define round_speed 5 | |
for (int degree = 0; degree < 360; degree += round_speed) | |
{ | |
move_body_absolute(round_length*sin(degree*PI / 180), round_length*cos(degree*PI / 180), 0); | |
} | |
return; | |
for (int degree = 360; degree >0; degree -= round_speed) | |
{ | |
move_body_absolute(round_length*sin(degree*PI / 180), round_length*cos(degree*PI / 180), 0); | |
} | |
for (int loop = 0; loop <3; loop++) | |
{ | |
for (int degree = 0; degree < 360; degree += round_speed) | |
{ | |
move_body_absolute(round_length*sin(degree*PI / 180), round_length*cos(degree*PI / 180), (degree / 360.0 + loop) * round_up); | |
} | |
} | |
for (int loop = 3; loop > 0; loop--) | |
{ | |
for (int degree = 0; degree < 360; degree += round_speed) | |
{ | |
move_body_absolute(round_length*sin(degree*PI / 180), round_length*cos(degree*PI / 180), ((360 - degree) / 360.0 + loop - 1) * round_up); | |
} | |
} | |
move_body_absolute(0, 0, 0); | |
} | |
/* | |
- sit | |
- blocking function | |
* ---------------------------------------------------------------------------*/ | |
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(); | |
} | |
/* | |
- rotate body by x axis function | |
* ---------------------------------------------------------------------------*/ | |
void rotate_body_absolute_x(float degree_x, float z) | |
{ | |
degree_x = degree_x * PI / 180; | |
float dz = (length_side / 2 + y_default)*sin(degree_x); | |
float dy = (length_side / 2 + y_default)*(1 - cos(degree_x)); | |
set_site(0, x_default, y_default - dy, z_default - z - dz); | |
set_site(1, x_default, y_default - dy, z_default - z + dz); | |
set_site(2, x_default, y_default - dy, z_default - z - dz); | |
set_site(3, x_default, y_default - dy, z_default - z + dz); | |
wait_all_reach(); | |
} | |
/* | |
- rotate body by y axis function | |
* ---------------------------------------------------------------------------*/ | |
void rotate_body_absolute_y(float degree_y, float z) | |
{ | |
degree_y = degree_y * PI / 180; | |
float dz = (length_side / 2 + x_default)*sin(degree_y); | |
float dx = (length_side / 2 + x_default)*(1 - cos(degree_y)); | |
set_site(0, x_default - dx, y_default, z_default - z + dz); | |
set_site(1, x_default - dx, y_default, z_default - z + dz); | |
set_site(2, x_default - dx, y_default, z_default - z - dz); | |
set_site(3, x_default - dx, y_default, z_default - z - dz); | |
wait_all_reach(); | |
} | |
/* | |
- move body function | |
* ---------------------------------------------------------------------------*/ | |
void move_body_relative(float dx, float dy, float dz) | |
{ | |
set_site(0, site_now[0][0] - dx, site_now[0][1] - dy, site_now[0][2] - dz); | |
set_site(1, site_now[1][0] - dx, site_now[1][1] + dy, site_now[1][2] - dz); | |
set_site(2, site_now[2][0] + dx, site_now[2][1] - dy, site_now[2][2] - dz); | |
set_site(3, site_now[3][0] + dx, site_now[3][1] + dy, site_now[3][2] - dz); | |
wait_all_reach(); | |
} | |
/* | |
- move body function | |
* ---------------------------------------------------------------------------*/ | |
void move_body_absolute(float x, float y, float z) | |
{ | |
set_site(0, x_default - x, y_default - y, z_default - z); | |
set_site(1, x_default - x, y_default + y, z_default - z); | |
set_site(2, x_default + x, y_default - y, z_default - z); | |
set_site(3, x_default + x, y_default + y, z_default - z); | |
wait_all_reach(); | |
} | |
/* | |
- adjustment function | |
- move each leg to adjustment site, so that you can measure the real sites. | |
* ---------------------------------------------------------------------------*/ | |
void adjust(void) | |
{ | |
//initializes eeprom's errors to 0 | |
//number -100 - +100 is map to 0 - +200 in eeprom | |
for (int i = 0; i < 4; i++) | |
{ | |
for (int j = 0; j < 3; j++) | |
{ | |
EEPROM.write(i * 6 + j * 2, 100); | |
EEPROM.write(i * 6 + j * 2 + 1, 100); | |
} | |
} | |
//initializes the relevant variables to adjustment position | |
for (int i = 0; i < 4; i++) | |
{ | |
set_site(i, adjust_site[0], adjust_site[1], adjust_site[2] + z_absolute); | |
for (int j = 0; j < 3; j++) | |
{ | |
site_now[i][j] = site_expect[i][j]; | |
} | |
} | |
//start servo service | |
if (use_polar) | |
FlexiTimer2::set(20, servo_service_polar); | |
else | |
FlexiTimer2::set(20, servo_service); | |
FlexiTimer2::start(); | |
//initialize servos | |
for (int i = 0; i < 4; i++) | |
{ | |
for (int j = 0; j < 3; j++) | |
{ | |
servo[i][j].attach(servo_pin[i][j]); | |
delay(100); | |
} | |
} | |
} | |
/* | |
- verify function | |
- verify the adjustment results, it will calculate errors and save to eeprom. | |
* ---------------------------------------------------------------------------*/ | |
void verify(void) | |
{ | |
//calculate correct degree | |
float alpha0, beta0, gamma0; | |
cartesian_to_polar(alpha0, beta0, gamma0, adjust_site[0], adjust_site[1], adjust_site[2] + z_absolute); | |
//calculate real degree and errors | |
float alpha, beta, gamma; | |
float degree_error[4][3]; | |
for (int i = 0; i < 4; i++) | |
{ | |
cartesian_to_polar(alpha, beta, gamma, real_site[i][0], real_site[i][1], real_site[i][2] + z_absolute); | |
degree_error[i][0] = alpha0 - alpha; | |
degree_error[i][1] = beta0 - beta; | |
degree_error[i][2] = gamma0 - gamma; | |
} | |
//save errors to eeprom | |
for (int i = 0; i < 4; i++) | |
{ | |
for (int j = 0; j < 3; j++) | |
{ | |
EEPROM.write(i * 6 + j * 2, (int)degree_error[i][j] + 100); | |
EEPROM.write(i * 6 + j * 2 + 1, (int)(degree_error[i][j] * 100) % 100 + 100); | |
} | |
} | |
//initializes the relevant variables to adjustment position | |
for (int i = 0; i < 4; i++) | |
{ | |
set_site(i, adjust_site[0], adjust_site[1], adjust_site[2] + z_absolute); | |
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 | |
for (int i = 0; i < 4; i++) | |
{ | |
for (int j = 0; j < 3; j++) | |
{ | |
servo[i][j].attach(servo_pin[i][j]); | |
delay(100); | |
} | |
} | |
} | |
/* | |
- 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); | |
} | |
} | |
/* | |
- 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_polar(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_polar[i][j] - site_expect_polar[i][j]) <= move_speed_polar) { | |
site_now_polar[i][j] = site_expect_polar[i][j]; | |
} else if ((site_expect_polar[i][j] - site_now_polar[i][j]) > 0) { | |
site_now_polar[i][j] += move_speed_polar; | |
} else { | |
site_now_polar[i][j] -= move_speed_polar; | |
} | |
} | |
alpha = site_now_polar[i][0]; | |
beta = site_now_polar[i][1]; | |
gamma = site_now_polar[i][2]; | |
polar_to_servo(i, alpha, beta, gamma); | |
} | |
} | |
/* | |
- 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; | |
} | |
/* | |
- set one of end points' expect site | |
- this founction will set temp_speed[4][3] at same time | |
- non - blocking function | |
* ---------------------------------------------------------------------------*/ | |
void set_site_polar(int leg, int x, int y, int z) | |
{ | |
site_expect_polar[leg][0] = x; | |
site_expect_polar[leg][1] = y; | |
site_expect_polar[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 one of end points move to one site | |
- blocking function | |
* ---------------------------------------------------------------------------*/ | |
void wait_reach(int leg, float x, float y, float z) | |
{ | |
while (1) | |
if (site_now[leg][0] == x) | |
if (site_now[leg][1] == y) | |
if (site_now[leg][2] == z) | |
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 polar to cartesian | |
- mathematical model 1/2 | |
* ---------------------------------------------------------------------------*/ | |
void polar_to_cartesian(volatile float alpha, volatile float beta, volatile float gamma, volatile float &x, volatile float &y, volatile float &z) | |
{ | |
//trans degree 180->PI | |
alpha = alpha / 180 * PI; | |
beta = beta / 180 * PI; | |
gamma = gamma / 180 * PI; | |
//calculate w-z position | |
float v, w; | |
v = length_a*cos(alpha) - length_b*cos(alpha + beta); | |
z = length_a*sin(alpha) - length_b*sin(alpha + beta); | |
w = v + length_c; | |
//calculate x-y-z position | |
x = w*cos(gamma); | |
y = w*sin(gamma); | |
} | |
/* | |
- 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) | |
{ | |
float alpha_error = EEPROM.read(leg * 6 + 0) - 100 + ((float)EEPROM.read(leg * 6 + 1) - 100) / 100; | |
float beta_error = EEPROM.read(leg * 6 + 2) - 100 + ((float)EEPROM.read(leg * 6 + 3) - 100) / 100; | |
float gamma_error = EEPROM.read(leg * 6 + 4) - 100 + ((float)EEPROM.read(leg * 6 + 5) - 100) / 100; | |
//Serial.println("leg " + String(leg) + " set to " + String(alpha) + " " + String(beta) + " " + String(gamma)); | |
alpha += alpha_error; | |
beta += beta_error; | |
gamma += gamma_error; | |
if (leg == 0) | |
{ | |
alpha = 90 - alpha; | |
beta = beta; | |
gamma = 90 - gamma; | |
} | |
else if (leg == 1) | |
{ | |
alpha += 90; | |
beta = 180 - beta; | |
gamma += 90; | |
} | |
else if (leg == 2) | |
{ | |
alpha += 90; | |
beta = 180 - beta; | |
gamma += 90; | |
} | |
else if (leg == 3) | |
{ | |
alpha = 90 - alpha; | |
beta = beta; | |
gamma = 90 - gamma; | |
} | |
if (servo[leg][0].attached()) | |
servo[leg][0].write(alpha); | |
if (servo[leg][1].attached()) | |
servo[leg][1].write(beta); | |
if (servo[leg][2].attached()) | |
servo[leg][2].write(gamma); | |
} | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment