/* sketch writed by nakamura android software please open this link : https://play.google.com/store/apps/details?id=braulio.calle.bluetoothRCcontroller demo video please open this link : https://drive.google.com/file/d/0B_eHmaTU40yFT0hTbXd2U3BvOE0/view?usp=sharing facebook: jordan chin gmail: nakamura6656353@gmail.com using twin 360 servo motor for track wheel and three 180 servo motor for robotic hand */ #include #include #include #define trigPin A1 #define echoPin A2 Servo myservo1; Servo myservo2; Servo myservo3; Servo myservo4; Servo myservo5; LiquidCrystal lcd(11, 8, 12, 9, 13, A4); int extra=0; int mode_change=0; //int poss = 5;int middle_pos = 35; int gripper_pos = 60; int led_time = 100; int opening = 0; int poss = 9;int middle_pos = 17; int gripper_pos = 60; int led_time = 100; int opening = 0; int const poss_max = 170; int const poss_min = 5; int displaying = 700; int reading = 500; int const middle_max = 170; int const middle_min = 17; int slowing_down = 5; int const gripper_max = 120; int const gripper_min = 55; int gripper_auto = 91; int brightness = 100; int lightpower = 0; const int time_data = 100; int time_count = 0; int headlight = 10; int backlights = A3; int position1 = 0; int position4 = 0; int position5 = 0; // Define Motor Speed Value int motorSpeed = 30; int turnSpeed = 4; int turning = 15; int posleft = 90; int posright = 90; int data1 = 90; //left int data2 = 90; //right int analogInput = A5; // conect to A5 to between R1 and R2 float vout = 0.0; float vin = 0.0; float R1 = 100000.0; // resistance of R1 (100K) - to + voltage! float R2 = 10000.0; // resistance of R2 (10K) - to - voltage! int value = 0; char val; // Define val To Store The Serial Input void setup() // Run once, when the sketch starts { lcd.begin(8, 2); //left right boom middle gripper myservo1.attach(2); myservo2.attach(3); myservo3.attach(4); myservo4.attach(5); myservo5.attach(6); Serial.begin(9600); // Set the baud rate to match with bluetooth pinMode(analogInput, INPUT); pinMode(headlight, OUTPUT); pinMode(backlights, OUTPUT); digitalWrite(headlight, HIGH); analogWrite(backlights, 70); pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT); lcd.setCursor(0, 0); lcd.print("Setting "); lcd.setCursor(0, 1); lcd.print("Up Servo"); myservo3.write(0); myservo4.write(middle_min); myservo5.write(gripper_min); delay(displaying); lcd.clear(); myservo1.write(data1); lcd.setCursor(0, 0); lcd.print("Servo 1 "); lcd.setCursor(0, 1); lcd.print(data1); lcd.print(" Deg"); delay(displaying); lcd.clear(); myservo2.write(data2); lcd.setCursor(0, 0); lcd.print("Servo 2 "); lcd.setCursor(0, 1); lcd.print(data2); lcd.print(" Deg"); delay(displaying); lcd.clear(); myservo3.write(poss); lcd.setCursor(0, 0); lcd.print("Servo 3 "); lcd.setCursor(0, 1); lcd.print(poss); lcd.print(" Deg"); delay(displaying); lcd.clear(); myservo4.write(middle_pos); lcd.setCursor(0, 0); lcd.print("Servo 4 "); lcd.setCursor(0, 1); lcd.print(middle_pos); lcd.print(" Deg"); delay(displaying); lcd.clear(); myservo5.write(gripper_pos); lcd.setCursor(0, 0); lcd.print("Servo 5 "); lcd.setCursor(0, 1); lcd.print(gripper_pos); lcd.print(" Deg"); delay(displaying); lcd.clear(); lcd.setCursor(0, 0); lcd.print("BlueTank"); lcd.setCursor(0, 1); lcd.print(" Ready! "); delay(displaying); lcd.clear(); digitalWrite(headlight, LOW); analogWrite(backlights, 0); } void voltage() { value = analogRead(analogInput); vout = (value * 5.0) / 1024.0; // see text vin = vout / (R2/(R1+R2)); if (vin<0.09) { vin=0.0; } } void light_effect() { if(extra==1){ /* lcd.setCursor(0, 0); lcd.print("Boom "); lcd.setCursor(0, 1); lcd.print("Servo"); lcd.print(poss);*/ } if(extra==2){ /* lcd.setCursor(0, 0); lcd.print("Arm Move"); lcd.setCursor(0, 1); lcd.print("Servo"); lcd.print(middle_pos);*/ } if(extra==3){ /* lcd.setCursor(0, 0); lcd.print("Bucket "); lcd.setCursor(0, 1); lcd.print("Servo"); lcd.print(gripper_pos);*/ } if(extra==4){ /* lcd.setCursor(0, 0); lcd.print("Setting "); lcd.setCursor(0, 1); lcd.print("Left "); lcd.print(data1);*/ } if(extra==5){ /* lcd.setCursor(0, 0); lcd.print("Setting "); lcd.setCursor(0, 1); lcd.print("Right"); lcd.print(data2);*/ } } void Turbo() { posleft=data1+motorSpeed; posright=data2-motorSpeed; myservo1.write(posleft); myservo2.write(posright); brightness = 12; } void backward() { posleft=data1-motorSpeed; posright=data2+motorSpeed; myservo1.write(posleft); myservo2.write(posright); brightness = 12; } void left() { posleft=data1-motorSpeed; posright=data2-motorSpeed; myservo1.write(posleft); myservo2.write(posright); brightness = 12; } void right() { posleft=data1+motorSpeed; posright=data2+motorSpeed; myservo1.write(posleft); myservo2.write(posright); brightness = 12; } void fowardleft() { if(motorSpeed<14){ turnSpeed=0; } else{ turnSpeed=motorSpeed-turning; } posleft=data1+turnSpeed; posright=data2-motorSpeed; myservo1.write(posleft); myservo2.write(posright); brightness = 12; } void fowardright() { if(motorSpeed<14){ turnSpeed=0; } else{ turnSpeed=motorSpeed-turning; } posleft=data1+motorSpeed; posright=data2-turnSpeed; myservo1.write(posleft); myservo2.write(posright); brightness = 12;} void backleft() { if(motorSpeed<14){ turnSpeed=0; } else{ turnSpeed=motorSpeed-turning; } posleft=data1-turnSpeed; posright=data2+motorSpeed; myservo1.write(posleft); myservo2.write(posright); brightness = 12;} void backright() { if(motorSpeed<14){ turnSpeed=0; } else{ turnSpeed=motorSpeed-turning; } posleft=data1-motorSpeed; posright=data2+turnSpeed; myservo1.write(posleft); myservo2.write(posright); brightness = 12;} void motorStop() { myservo1.write(data1); myservo2.write(data2); brightness = 100;} void headlighton() { digitalWrite(headlight, HIGH);} void headlightoff() { digitalWrite(headlight, LOW); } void servoleft() { myservo3.write(poss); // tell servo to go to position in variable 'pos' poss++; delay(15); if (poss>poss_max){ poss=poss_max; } } void servoright() { myservo3.write(poss); // tell servo to go to position in variable 'pos' poss--; delay(15); if(possmiddle_max){ middle_pos=middle_max; } } void middle_servo_down() { myservo4.write(middle_pos); // tell servo to go to position in variable 'pos' middle_pos--; delay(15); if(middle_posgripper_max){ gripper_pos=gripper_max; } } void gripper_servo_down() { myservo5.write(gripper_pos); // tell servo to go to position in variable 'pos' gripper_pos--; delay(15); if(gripper_posgripper_max){gripper_auto=gripper_max;} } void loop() // Run continuously until powered down========================== { // if(time_count16)) { posleft=data1+slowing_down; posright=data2+slowing_down; myservo1.write(posleft); myservo2.write(posright); myservo3.write(poss_min); myservo4.write(40); delay(1000); } if((distance<28)&&(distance>22)) { posleft=data1+slowing_down; posright=data2-slowing_down; myservo1.write(posleft); myservo2.write(posright); myservo3.write(poss_min); myservo4.write(40); } if(distance>40) { posleft=data1+motorSpeed; posright=data2-motorSpeed; myservo1.write(posleft); myservo2.write(posright); myservo3.write(poss_min); myservo4.write(40); } } if(extra==7){ long duration, distance; digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); duration = pulseIn(echoPin, HIGH); distance = (duration/2) / 29.1; lcd.setCursor(0, 0); lcd.print("Capture "); lcd.setCursor(0, 1); lcd.print("Down Mod"); if(distance<6) { gripper_pos = gripper_auto; myservo1.write(data1); myservo2.write(data2); myservo5.write(gripper_pos); delay(200); myservo3.write(poss_min); delay(200); myservo4.write(17); } if((distance<8)&&(distance>6)) { posleft=data1+slowing_down; posright=data2-slowing_down; myservo1.write(posleft); myservo2.write(posright); myservo3.write(80); myservo4.write(60); myservo5.write(50); } if(distance>8) { posleft=data1+motorSpeed; posright=data2-motorSpeed; myservo1.write(posleft); myservo2.write(posright); myservo3.write(80); myservo4.write(60); myservo5.write(95); } } if(extra==8){ long duration, distance; digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); duration = pulseIn(echoPin, HIGH); distance = (duration/2) / 29.1; lcd.setCursor(0, 0); lcd.print("Capture "); lcd.setCursor(0, 1); lcd.print("Auto Mod"); //Serial.println(position1); if(position1==0){ if(distance>14){ poss = 5; gripper_pos = 50; myservo3.write(poss); myservo4.write(middle_pos); myservo5.write(gripper_pos); middle_pos++; delay(50); if(middle_pos>75){ middle_pos = 17; } }// SEARCHING if((distance<14)&&(distance>4)){//DETECT myservo3.write(poss); myservo4.write(middle_pos); middle_pos=middle_pos+2; poss++; if(middle_pos>middle_max){ middle_pos = 170;} if(poss>120){poss = 120;} } if(distance<4){gripper_pos = gripper_auto; myservo5.write(gripper_pos); delay(500); extra = 0; poss = poss_min; middle_pos = middle_min; myservo3.write(poss_min); myservo4.write(middle_min); } } } if(extra==9){ lcd.setCursor(0, 0); lcd.print("Gripper "); lcd.setCursor(0, 1); lcd.print("Set: "); lcd.setCursor(5, 1); lcd.print(gripper_auto); if(gripper_auto<100){ lcd.setCursor(7, 1); lcd.print(" "); } if(gripper_auto<10){ lcd.setCursor(6, 1); lcd.print(" "); } } if(lightpower==1){ analogWrite(backlights, brightness); } if(lightpower==0){ analogWrite(backlights, 0); } if(extra==0){ mode_change = 0; position1 = 0; voltage(); lcd.setCursor(0, 0); lcd.print("Battery "); lcd.setCursor(0, 1); if(time_count