/* Arduino Jooystick-Controlled and self driving Car Using Arduino Mega 2560 and Motor Shield L293d Leds in serie connected to 14 Servos: Right attatches to the pin 10 Left attatches to the pin 9 Ultrasonic Sensors: Middle: Trig: 16 Echo: 15 Right: Trig: A2 Echo: A3 Left: Trig: A0 Echo: A1 Motors: ((Seen from the side of where the gnd and 5v connect to the motor shield L293d)) M1: Orange, Black M2: Blue, Black M3: Orange, White M4: White, Blue Shield is powered with 6 AA PS2 Jack for Joystick: (Seen form the front) (Data, Command, GND, VCC, SEL, Clock) Data: 47 Command: 49 SEL: 51 Clock: 53 How to use the car: Press Start to turn On or Off Press SELECT to change from Joystick Controlled to Autonomous When controlled: X = Back O = Left /\ = FORWARD |_| = RIGHT R2 = TURN RIGHT L2 = TURN LEFT R1 = SPEED BOOST Left Joystick: Drive as in any video game Also, when pressing the shape buttons, the harder you press,the more it will vibrate. Enjoy! */ #include #include #include //for v1.6 #include Servo servo; Servo servo2; int lim_vis = 45; NewPing sonar (A2,A3,lim_vis); //se mueve NewPing sonar2 (A0,A1,lim_vis); //estatico NewPing sonar3 (16,15,lim_vis); //estatico AF_DCMotor motor1(1); AF_DCMotor motor2(2); AF_DCMotor motor3(3); AF_DCMotor motor4(4); #define PS2_DAT 47 //14 #define PS2_CMD 49 //15 #define PS2_SEL 51 //16 #define PS2_CLK 53 #define pressures true #define rumble true PS2X ps2x; // create PS2 Controller Class int error = 0; byte type = 0; byte vibrate = 0; bool mode = true; bool on = true; bool leds = true; int vel = 180; int a ; int b ; int f ; int e ; int h ; int pos ; int c = 0; int rate = 18; int posmov1 = 2; int posmov2 = 175; int angmax = 100; int halfmax = angmax/2; int angmin = 0; int joa(){ j(a); if (pos == 1){ a = a - 1; a = k(a); } else{ a = a + 1; a = v(a); } b = 180 - a; b = map (b,180-angmax,180-angmin,180-angmin,180-angmax); return pos; return b; } int j(int a){ if(a == angmax){ delay(150); pos = 1; } if(a == angmin){ delay(100); pos = 0; } return pos; } int k(int a){ if(a == 60){ // 180-175 a = angmin; } return a; } int v(int a){ if(a == 40){ // 180-175 a = angmax; } return a; } int div_factor = 6; int d1 = 500; int d2 = 200; void go_back() { motor1.run(BACKWARD); motor2.run(BACKWARD); motor3.run(BACKWARD); motor4.run(BACKWARD); } void advance() { motor1.run(FORWARD); motor2.run(FORWARD); motor3.run(FORWARD); motor4.run(FORWARD); } void just_chill_baby(){ motor1.run(RELEASE); motor2.run(RELEASE); motor3.run(RELEASE); motor4.run(RELEASE); } void turn_right() { motor1.run(RELEASE); motor2.run(BACKWARD); motor3.run(BACKWARD); motor4.run(RELEASE); } void turn_back_right() { motor1.run(RELEASE); motor2.run(FORWARD); motor3.run(FORWARD); motor4.run(RELEASE); } void turn_left() { motor1.run(BACKWARD); motor2.run(RELEASE); motor3.run(RELEASE); motor4.run(BACKWARD); } void turn_back_left() { motor1.run(FORWARD); motor2.run(RELEASE); motor3.run(RELEASE); motor4.run(FORWARD); } void rotate_left(){ motor1.run(BACKWARD); motor2.run(FORWARD); motor3.run(FORWARD); motor4.run(BACKWARD); } void rotate_right(){ motor1.run(FORWARD); motor2.run(BACKWARD); motor3.run(BACKWARD); motor4.run(FORWARD); } void setup(){ servo.attach(9); servo2.attach(10); servo.write(0); servo2.write(180); pinMode(14,OUTPUT); digitalWrite(14,HIGH); motor1.setSpeed(vel); motor2.setSpeed(vel); motor3.setSpeed(vel); motor4.setSpeed(vel); Serial.begin(9600); delay(300); //added delay to give wireless ps2 module some time to startup, before configuring it error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble); if(error == 0){ Serial.print("Found Controller, configured successful "); Serial.print("pressures = "); if (pressures) Serial.println("true "); else Serial.println("false"); Serial.print("rumble = "); if (rumble) Serial.println("true)"); else Serial.println("false"); Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;"); Serial.println("holding L1 or R1 will print out the analog stick values."); Serial.println("Note: Go to www.billporter.info for updates and to report bugs."); } else if(error == 1) Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips"); else if(error == 2) Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips"); else if(error == 3) Serial.println("Controller refusing to enter Pressures mode, may not support it. "); type = ps2x.readType(); switch(type) { case 0: Serial.print("Unknown Controller type found "); break; case 1: Serial.print("DualShock Controller found "); break; case 2: Serial.print("GuitarHero Controller found "); break; case 3: Serial.print("Wireless Sony DualShock Controller found "); break; } } void loop() { if(error == 1) //skip loop if no controller found return; else{ ps2x.read_gamepad(false, vibrate); if (ps2x.ButtonPressed(PSB_START)){ on = !on; } if (ps2x.ButtonPressed(PSB_SELECT)){ mode = !mode; } if (on == true){ if (mode == true){ if (c == 0){ servo.write(0); servo2.write(175); c += 1; } if(ps2x.Button(PSB_PAD_UP)) { //will be TRUE as long as button is pressed Serial.print("Up held this hard: "); Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC); } //this will set the large motor vibrate speed based on how hard you press the blue (X) button if (ps2x.NewButtonState()) { //will be TRUE if any button changes state (on to off, or off to on) if(ps2x.Button(PSB_L3)) Serial.println("L3 pressed"); } if ((ps2x.Analog(PSS_RX) > 150)) { posmov1 -= rate; posmov2 += rate; if(posmov1 > 180){ posmov1 = 180; } if(posmov2 > 180){ posmov2 = 180; } if(posmov1 < 0){ posmov1 = 0; } if(posmov2 < 0){ posmov2 = 0; } servo.write(posmov1); servo2.write(posmov2); } if ((ps2x.Analog(PSS_RX) < 30)){ posmov1 += rate; posmov2 -= rate; if(posmov1 > 180){ posmov1 = 180; } if(posmov2 > 180){ posmov2 = 180; } if(posmov1 < 0){ posmov1 = 0; } if(posmov2 < 0){ posmov2 = 0; } servo.write(posmov1); servo2.write(posmov2); } if(ps2x.ButtonPressed(PSB_R2)){ //will be TRUE if button was JUST pressed vel = 250;} if(ps2x.ButtonReleased(PSB_R2)){ //will be TRUE if button was JUST pressed vel = 180;} if(ps2x.ButtonPressed(PSB_L2)){ //will be TRUE if button was JUST pressed leds = !leds;} if ((ps2x.Analog(PSS_LY) > 150) && (ps2x.Analog(PSS_LX) < 106)){ turn_back_left(); } if ((ps2x.Analog(PSS_LX) < 106) && (ps2x.Analog(PSS_LY) < 106)){ turn_left(); } if((ps2x.Analog(PSS_LX) < 106) && (ps2x.Analog(PSS_LY) < 150) && (ps2x.Analog(PSS_LY) > 106)){ rotate_left(); } if ((ps2x.Analog(PSS_LY) > 150) && (ps2x.Analog(PSS_LX) < 150) && (ps2x.Analog(PSS_LX) > 106)){ advance(); } if ((ps2x.Analog(PSS_LY) < 106) && (ps2x.Analog(PSS_LX) < 150) && (ps2x.Analog(PSS_LX) > 106)){ go_back(); } if ((ps2x.Analog(PSS_LX) > 150) && (ps2x.Analog(PSS_LY) < 106)){ turn_right(); } if ((ps2x.Analog(PSS_LX) > 150) && (ps2x.Analog(PSS_LY) > 150)){ turn_back_right(); } if ((ps2x.Analog(PSS_LX) > 150) && (ps2x.Analog(PSS_LY) < 150) && (ps2x.Analog(PSS_LY) > 106)){ rotate_right(); } if ((ps2x.Analog(PSS_LX) < 150) &&(ps2x.Analog(PSS_LX) > 106) && (ps2x.Analog(PSS_LY) < 150) && (ps2x.Analog(PSS_LY) > 106)){ if(ps2x.ButtonPressed(PSB_CIRCLE)){ //will be TRUE if button was JUST pressed rotate_right(); vibrate = ps2x.Analog(PSAB_CIRCLE);} if(ps2x.ButtonPressed(PSB_TRIANGLE)){ //will be TRUE if button was JUST pressed OR released go_back(); vibrate = ps2x.Analog(PSAB_TRIANGLE);} if(ps2x.ButtonPressed(PSB_SQUARE)){ //will be TRUE if button was JUST released rotate_left(); vibrate = ps2x.Analog(PSAB_SQUARE);} if(ps2x.ButtonPressed(PSB_CROSS)){ //will be TRUE if button was JUST released advance(); vibrate = ps2x.Analog(PSAB_CROSS);} if(ps2x.ButtonPressed(PSB_L1)){ turn_left();} if(ps2x.ButtonPressed(PSB_R1)){ turn_right();} if(ps2x.ButtonReleased(PSB_CIRCLE)){ //will be TRUE if button was JUST pressed just_chill_baby();} if(ps2x.ButtonReleased(PSB_CROSS)){ //will be TRUE if button was JUST pressed OR released just_chill_baby();} if(ps2x.ButtonReleased(PSB_SQUARE)){ //will be TRUE if button was JUST released just_chill_baby();} if(ps2x.ButtonReleased(PSB_TRIANGLE)){ //will be TRUE if button was JUST released just_chill_baby();} if(ps2x.ButtonReleased(PSB_L1)){ just_chill_baby();} if(ps2x.ButtonReleased(PSB_R1)){ just_chill_baby();} if((!ps2x.Button(PSB_R1)) && (!ps2x.Button(PSB_L1)) && (!ps2x.Button(PSB_TRIANGLE)) && (!ps2x.Button(PSB_SQUARE)) && (!ps2x.Button(PSB_CROSS)) && (!ps2x.Button(PSB_CIRCLE))){ just_chill_baby(); } if((!ps2x.Button(PSB_TRIANGLE)) && (!ps2x.Button(PSB_SQUARE)) && (!ps2x.Button(PSB_CROSS)) && (!ps2x.Button(PSB_CIRCLE))){ vibrate = false; } } if(leds == true){ digitalWrite(14,LOW); }else{ digitalWrite(14,HIGH); } delay(30); }else if (mode == false){ c = 0; vel = 220; joa(); servo.write(a); servo2.write(b); if (a % div_factor == 0){ // que solo tome medidas y de cada div_factor f = sonar.ping_cm(); h = sonar2.ping_cm(); e = sonar3.ping_cm(); } switch (a){ case 0 ... 50: // si detecta algo al costado o delante, rota while ((h > 0 && h < lim_vis) && (f > 0 && f < lim_vis) && (e > 0 && e < lim_vis)) { advance(); delay(d1); rotate_left(); delay(d1); f = sonar.ping_cm(); h = sonar2.ping_cm(); e = sonar3.ping_cm(); } while (((h > 0 && h < lim_vis) || (f > 0 && f < lim_vis)) && (e == 0 || e > lim_vis)) { rotate_right(); f = sonar.ping_cm(); h = sonar2.ping_cm(); e = sonar3.ping_cm(); delay(d2); } while (((h > 0 && h < lim_vis) || (f > 0 && f < lim_vis)) && (e > 0 && e < lim_vis)) { rotate_left(); f = sonar.ping_cm(); h = sonar2.ping_cm(); e = sonar3.ping_cm(); delay(d2); } while ((e > 0 && e < lim_vis) && (h > lim_vis || h == 0)){ rotate_left(); f = sonar.ping_cm(); h = sonar2.ping_cm(); e = sonar3.ping_cm(); delay(d2); } go_back(); break; case 51 ... 101: // si detecta algo al costado o delante, rota while ((h > 0 && h < lim_vis) && (e > 0 && e < lim_vis) && (f > 0 && f < lim_vis)) { advance(); delay(d1); rotate_left(); delay(d1); f = sonar.ping_cm(); h = sonar2.ping_cm(); e = sonar3.ping_cm(); } while (((h > 0 && h < lim_vis) || (e > 0 && e < lim_vis)) && (f == 0 || f > lim_vis)) { rotate_left(); f = sonar.ping_cm(); h = sonar2.ping_cm(); e = sonar3.ping_cm(); delay(d2); } while (((h > 0 && h < lim_vis) || (e > 0 && e < lim_vis)) && (f > 0 && f < lim_vis)) { rotate_right(); f = sonar.ping_cm(); h = sonar2.ping_cm(); e = sonar3.ping_cm(); delay(d2); } while ((f > 0 && f < lim_vis) && (h > lim_vis || h == 0)){ rotate_right(); f = sonar.ping_cm(); h = sonar2.ping_cm(); e = sonar3.ping_cm(); delay(d2); } go_back(); break; } } motor1.setSpeed(vel); motor2.setSpeed(vel); motor3.setSpeed(vel); motor4.setSpeed(vel); } } }