Diff: main.cpp

Revision:

0:901a3ec83ba4

Child:

1:1eb3a5a00fbb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jun 03 04:56:59 2021 +0000
@@ -0,0 +1,50 @@
+/*******************************************************
+              DC MODOR CONTROLLER LIBRARY
+ *******************************************************           
+         IMPORTANT: GO TO README FOR DOCUMENT!
+********************************************************/
+
+#include "mbed.h"
+#include "LSCServo.h"
+#include "DC_Motor_Controller.h"
+
+
+DC_Motor_PID motor(D6,D3,D4,D5,792); //M1, M2, INA, INB, PPR 
+DigitalIn but(D7);
+LSCServo robot_arm(D1, D0); //D1 = Tx, D0 = Rx
+
+int main() {
+    
+    motor.set_pid(0.008, 0, 0, 0.10);
+
+    but.mode(PullUp);
+    
+    
+    wait(0.5);motor.set_out(0.4,0);
+    while(1)
+    {
+            if(but) { motor.set_out(0,0); break;}
+    }
+    
+    wait(0.5);
+    motor.reset();
+    wait(0.5);
+    motor.move_angle(615);
+    wait(2);
+    
+    robot_arm.moveServos(3,1000,2,185,3,560,4,0); // initial pos to put an arrow
+    wait(2);
+    
+    robot_arm.moveServos(3,1000,2,185,3,560,4,800); // catch/crawl the arror
+    robot_arm.moveServos(3,1000,2,185,3,190,4,800); // rotate down
+    wait(2);
+    robot_arm.moveServos(3,1500,2,760,3,190,4,800); // rotate left
+    wait(2);
+    
+    robot_arm.moveServos(3,1000,2,580,3,190,4,0); // open the crawler
+    //robot_arm.moveServos(3,1000,2,600,3,190,4,0); // rotate a little back
+    motor.move_angle(420);
+    
+    robot_arm.moveServos(3,1000,2,185,3,560,4,0); // reset to init
+    
+}
\ No newline at end of file