Robocon 2021 / Mbed 2 deprecated DR-ArmServoTest

Union test running for robot arm servos & motor

Dependencies:   mbed BufferedSerial LSCServo DC_Stepper_Controller_Lib

main.cpp@1:1eb3a5a00fbb, 2021-06-03 (annotated)

Committer:
stivending
Date:
Thu Jun 03 07:42:11 2021 +0000
Revision:
1:1eb3a5a00fbb
Parent:
0:901a3ec83ba4 
working @ 3pm Jun 3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stivending 0:901a3ec83ba4 1
stivending 0:901a3ec83ba4 2 #include "mbed.h"
stivending 0:901a3ec83ba4 3 #include "LSCServo.h"
stivending 0:901a3ec83ba4 4 #include "DC_Motor_Controller.h"
stivending 0:901a3ec83ba4 5
stivending 0:901a3ec83ba4 6
stivending 0:901a3ec83ba4 7 DC_Motor_PID motor(D6,D3,D4,D5,792); //M1, M2, INA, INB, PPR
stivending 0:901a3ec83ba4 8 DigitalIn but(D7);
stivending 0:901a3ec83ba4 9 LSCServo robot_arm(D1, D0); //D1 = Tx, D0 = Rx
stivending 0:901a3ec83ba4 10
stivending 0:901a3ec83ba4 11 int main() {
stivending 0:901a3ec83ba4 12
stivending 0:901a3ec83ba4 13 motor.set_pid(0.008, 0, 0, 0.10);
stivending 0:901a3ec83ba4 14
stivending 0:901a3ec83ba4 15 but.mode(PullUp);
stivending 0:901a3ec83ba4 16
stivending 0:901a3ec83ba4 17
stivending 0:901a3ec83ba4 18 wait(0.5);motor.set_out(0.4,0);
stivending 0:901a3ec83ba4 19 while(1)
stivending 0:901a3ec83ba4 20 {
stivending 0:901a3ec83ba4 21 if(but) { motor.set_out(0,0); break;}
stivending 0:901a3ec83ba4 22 }
stivending 0:901a3ec83ba4 23
stivending 0:901a3ec83ba4 24 wait(0.5);
stivending 1:1eb3a5a00fbb 25 //motor.reset();
stivending 0:901a3ec83ba4 26 wait(0.5);
stivending 1:1eb3a5a00fbb 27 motor.move_angle(620);
stivending 0:901a3ec83ba4 28 wait(2);
stivending 0:901a3ec83ba4 29
stivending 1:1eb3a5a00fbb 30 for(int i =0; i < 5; i++)
stivending 1:1eb3a5a00fbb 31 {
stivending 1:1eb3a5a00fbb 32
stivending 1:1eb3a5a00fbb 33 robot_arm.moveServos(3,1000,2,185,3,560,4,0); // initial pos to put an arrow
stivending 1:1eb3a5a00fbb 34 wait(2);
stivending 1:1eb3a5a00fbb 35
stivending 1:1eb3a5a00fbb 36 robot_arm.moveServos(3,1000,2,185,3,560,4,800); // catch/crawl the arror
stivending 1:1eb3a5a00fbb 37 robot_arm.moveServos(3,1000,2,185,3,190,4,800); // rotate down
stivending 1:1eb3a5a00fbb 38 wait(2);
stivending 1:1eb3a5a00fbb 39 robot_arm.moveServos(3,1500,2,760,3,190,4,800); // rotate left
stivending 1:1eb3a5a00fbb 40 wait(2);
stivending 1:1eb3a5a00fbb 41
stivending 1:1eb3a5a00fbb 42 robot_arm.moveServos(3,1000,2,560,3,190,4,0); // open the crawler
stivending 1:1eb3a5a00fbb 43 //robot_arm.moveServos(3,1000,2,600,3,190,4,0); // rotate a little back
stivending 1:1eb3a5a00fbb 44 motor.move_angle(412);
stivending 1:1eb3a5a00fbb 45
stivending 1:1eb3a5a00fbb 46 robot_arm.moveServos(3,1000,2,185,3,560,4,0); // reset to init
stivending 1:1eb3a5a00fbb 47 }
stivending 1:1eb3a5a00fbb 48
stivending 0:901a3ec83ba4 49 }