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 #include "nRF24L01.h" #include "RF24.h" #include #include #define CE_PIN 7 #define CSN_PIN 8 const uint64_t pipe = 0xE8E8F0F0E1LL; R

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);

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)); }