Code phần nhận di chuyển robot nhện #include #include "nRF24L01.h" #include "RF24.h" #include #include #define CE_PIN #define CSN_PIN 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)); } ... 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);... 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);... pwm.setPWMFreq(FREQUENCY); radio.begin(); radio.setChannel(115); radio.setPALevel(RF 24_ PA_MAX); radio.setDataRate( RF 24_ 250KBPS ) ; radio.openReadingPipe(1,pipe); radio.startListening(); } int