Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Servo.h>
- #include <WemosInit.h> //Custom library - ultrasonicPing()
- #define motor1pin 12 //GPIO pin setting for motor1
- #define motor2pin 2 //GPIO pin setting for motor2
- Servo motor1; //Create servo motor1 object to control a servo, right wheel
- Servo motor2; //Create servo motor2 object to control a servo
- double distance;
- void setup() //put your setup code here, to run once:
- {
- Serial.begin(115200); //to use the Serial Monitor
- pinMode(D2, OUTPUT); //Trigger - Sends out ultranonic wave
- pinMode(D3, INPUT); //Echo - Receives return signal
- motor1.attach(motor1pin); //motor1 is attached using the motor1pin
- motor2.attach(motor2pin); //motor2 is attached using the motor2pin
- motor1.write(0); //motor1 rotates with a full speed
- motor2.write(0); //motor2 rotates with a full speed
- }
- void loop()
- {
- delay(1000);
- //Starting Run
- motor1.write(0);
- motor2.write(0);
- delay(1200);
- //First Turn
- motor1.write(0);
- motor2.write(90);
- delay(200);
- //Straightaway before Target 1
- motor1.write(0);
- motor2.write(0);
- delay(1100);
- //Backup after target 1
- motor1.write(180);
- motor2.write(180);
- delay(700);
- //Turn back after target 1
- motor1.write(0);
- motor2.write(90);
- delay(250);
- //Second Straightaway
- motor1.write(0);
- motor2.write(0);
- delay(1925);
- //Turn to target 2
- motor1.write(0);
- motor2.write(90);
- delay(350);
- //Move towards target 2
- motor1.write(0);
- motor2.write(0);
- delay(850);
- //Back up to target 3
- motor1.write(180);
- motor2.write(180);
- delay(1975);
- //Move forward after target 3
- motor1.write(0);
- motor2.write(0);
- delay(100);
- //Turn after target 3
- motor1.write(0);
- motor2.write(90);
- delay(525);
- //Movement towards target 4
- motor1.write(0);
- motor2.write(0);
- delay(1650);
- //Turn perpendicular to target 4
- motor1.write(90);
- motor2.write(0);
- delay(400);
- //Hit target 4
- motor1.write(0);
- motor2.write(0);
- delay(925);
- //Reverse from target 4
- motor1.write(180);
- motor2.write(180);
- delay(600);
- //Turn towards start
- motor1.write(0);
- motor2.write(90);
- delay(650);
- //Run towards start
- motor1.write(0);
- motor2.write(0);
- delay(500);
- /* motor1.write(0);
- motor2.write(90);
- delay(750);
- motor1.write(0);
- motor2.write(0);
- delay(3500);
- motor1.write(90);
- motor2.write(0);
- delay(500);
- motor1.write(0);
- motor2.write(0);
- delay(700);
- */
- motor1.write(90);
- motor2.write(90);
- delay(99999);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement