Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
/* -----------------------------------------------------------------------------
- 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