Code phần nhận và di chuyển của robot nhện 4 chân

Code phần nhận và di chuyển của robot nhện
#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#define CE_PIN 7
#define CSN_PIN 8
const uint64_t pipe = 0xE8E8F0F0E1LL;
RF24 radio(CE_PIN, CSN_PIN);
int joystick[6];
int xAxis ,yAxis;
int buttonUp

;

int buttonRight ;
int buttonDown ;
int buttonLeft ;
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define MIN_PULSE_WIDTH 544
#define MAX_PULSE_WIDTH 2400
#define DEFAULT_PULSE_WIDTH 1500
#define FREQUENCY 50
void setup() {
pinMode(buttonUp,INPUT_PULLUP);
pinMode(buttonRight,INPUT_PULLUP);
pinMode(buttonDown,INPUT_PULLUP);
pinMode(buttonLeft,INPUT_PULLUP);
digitalWrite(buttonUp,LOW);

digitalWrite(buttonRight,LOW);
digitalWrite(buttonDown,LOW);
digitalWrite(buttonLeft,LOW);
Serial.begin(9600);
// Serial.println("16 channel Servo test!");
pwm.begin();
pwm.setPWMFreq(FREQUENCY);
radio.begin();
radio.setChannel(115);
radio.setPALevel(RF24_PA_MAX);
radio.setDataRate( RF24_250KBPS ) ;
radio.openReadingPipe(1,pipe);
radio.startListening();

}
int pulseWidth(int angle)
{
int pulse_wide, analog_value;
pulse_wide = map(angle, 0, 180,
MIN_PULSE_WIDTH,MAX_PULSE_WIDTH);
// Serial.println(pulse_wide);
analog_value = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
// Serial.println(analog_value);
return analog_value;
}


void loop()
{

if ( radio.available() )
{radio.read( joystick, sizeof(joystick) );
xAxis = joystick[0] ;
yAxis = joystick[1] ;
int buttonUp

= joystick[2];

int buttonRight = joystick[3];
int buttonDown = joystick[4];
int buttonLeft = joystick[5];
Serial.print("X = ");
Serial.print(xAxis);
Serial.print(" Y = ");
Serial.print(yAxis);
Serial.print(" Up = ");
Serial.print(joystick[2]);
Serial.print(" Right = ");
Serial.print(joystick[3]);
Serial.print(" Down = ");
Serial.print(joystick[4]);
Serial.print(" Left = ");
Serial.println(joystick[5]);
if (joystick[2]==1&&joystick[3]==1 &&
joystick[4]==1&&joystick[5]==1 )
dungyen();


if (joystick[2]==0 )

tien1();
if (joystick[3]==0 )
phai1();
if (joystick[5]==0 )
trai1();
if (joystick[4]==0 )
lui1();
}
}
void dungyen(){
chan1(140,120,90);
chan2(140,120,90);
chan3(140,120,90);
chan4(140,120,90);
}
void tien1(){
chan1(140,130,70);
delay(300);
chan3(140,180,70);
chan2(140,130,90);
chan4(140,130,90);
delay(300);
chan1(140,180,90);
chan3(140,130,90);
delay(300);
chan2(140,180,60);


chan4(140,180,60);
chan1(140,130,90);

chan3(140,130,90);
delay(300);
chan2(140,180,90);
chan4(140,180,90);
}
void phai1(){
chan3(140,180,110);
delay(300);
chan3(140,130,110);
delay(300);
chan4(140,180,50);
delay(300);
chan4(140,130,50);
delay(300);
chan2(140,180,50);
delay(300);
chan2(140,120,50);
delay(300);
chan1(140,180,120);
delay(300);
chan1(140,120,120);
delay(300);
}
void trai1(){
chan4(140,180,120);
delay(300);


chan4(140,130,120);
delay(300);

chan3(140,180,50);
delay(300);
chan3(140,130,50);
delay(300);
chan1(140,180,50);
delay(300);
chan1(140,120,50);
delay(300);
chan2(140,180,120);
delay(300);
chan2(140,120,120);
delay(300);
}
void lui1(){
chan1(140,180,60);
delay(300);
chan1(140,120,60);
delay(300);
chan2(140,180,60);
delay(300);
chan2(140,130,60);
delay(300);
chan3(140,140,90);
chan4(140,140,90);
delay(300);
chan3(140,180,110);
delay(300);


chan3(140,130,110);

delay(300);
chan4(140,180,110);
delay(300);
chan4(140,130,110);
delay(300);
chan3(120,130,90);
chan4(120,130,90);
delay(300);
chan1(160,120,90);
chan2(160,120,90);
delay(300);
chan1(140,180,90);
chan2(140,180,90);
delay(300);
}
void chan1(int a, int b, int c){
int x;
int y;
int z;
x=85+(a-90);//80
y=110+(b-90);//75
z=110+(c-90);//90
pwm.setPWM(0,0,pulseWidth(x));
pwm.setPWM(1,0,pulseWidth(y));
pwm.setPWM(2,0,pulseWidth(z));
}


void chan2(int a, int b, int c){
int x;

int y;
int z;
x=93-(a-90);
y=95-(b-90);
z=70-(c-90);
pwm.setPWM(3,0,pulseWidth(x));
pwm.setPWM(4,0,pulseWidth(y));
pwm.setPWM(5,0,pulseWidth(z));
}
void chan3(int a, int b, int c){
int x;
int y;
int z;
x=95+(a-90);
y=92+(b-90);
z=85+(c-90);
pwm.setPWM(6,0,pulseWidth(x));
pwm.setPWM(7,0,pulseWidth(y));
pwm.setPWM(8,0,pulseWidth(z));
}
void chan4(int a, int b, int c){
int x;
int y;
int z;
x=80-(a-90);
y=100-(b-90);


z=95-(c-90);
pwm.setPWM(9,0,pulseWidth(x));

pwm.setPWM(10,0,pulseWidth(y));
pwm.setPWM(11,0,pulseWidth(z));
}