Diff: main.cpp

Revision:

2:55f39e7883a6

Parent:

0:ba8a9d66892d

--- a/main.cpp	Fri Feb 17 00:00:55 2017 +0000
+++ b/main.cpp	Fri Jul 14 12:32:18 2017 +0000
@@ -1,41 +1,76 @@
 #include "robotarm.h"
+#include <math.h>
+#include "serial.h"
+#define ON 1
+#define OFF 0
 
 Robotarm arm;
+DigitalOut beacon(p15);
+SerialControl serial;
+
 
 int main()
-{
-    wait(1); // Useful if you need to connect H-Term etc.
+{   
+    
+    int base_degree = 90;    
+    int flag = 1;
+    
     //Run the main initialisation routine
-    arm.init();
+    arm.init();  
+    serial.setup_serial_interfaces();
+    /* Set all servos speed to 0.1 */
+    servo.SetCRSpeed( BASE , 0.1 );
+    servo.SetCRSpeed( SHOULDER , 0.1 );
+    servo.SetCRSpeed( ELBOW , 0.1 );
+    
     //Reset the servos to center position (after 1 second delay)
     //NB This activates the servos (makes rigid) so be careful when using
     arm.zero_servos(1);
     //Wait till servos are zeroed
     wait(3);
-    //Initialise remote control
-    if(REMOTE_ENABLED == 1)remote.init();
-    
-    //User code can now go in a loop:
-    while(1) {
-        //Eg set all servos to 1948 then 2148
-          servo.SetGoal(BASE,1948,1);
-          servo.SetGoal(SHOULDER,1948,1);
-          servo.SetGoal(ELBOW,1948,1);
-          servo.SetGoal(WRIST,400,1);
-            //If you want to show detailed info about a servo over serial, use the following:
-            //servo.DebugData(WRIST);
-          wait(0.5);
-          servo.SetGoal(BASE,2148,1);
-          servo.SetGoal(SHOULDER,2148,1);
-          servo.SetGoal(ELBOW,2148,1);
-          servo.SetGoal(WRIST,600,1);
-          wait(0.5);
-            //Alternatively we can set all the servos then use trigger - observe the difference...
-            //servo.SetGoal(BASE,2148,0);
-            //servo.SetGoal(SHOULDER,2148,0);
-            //servo.SetGoal(ELBOW,2148,0);
-            //servo.SetGoal(WRIST,600,0);
-            //servo.trigger();
-            //wait(0.5);
+    display.clear_display();
+    display.set_position(0,0);
+    display.write_string("start"); 
+    servo.SetGoalDegrees( BASE , 0 , 1);
+    servo.SetGoalDegrees( SHOULDER , 0 , 1 );
+    servo.SetGoalDegrees( ELBOW , 90 , 1 );
+    while(1){
+        if(turning == 1){
+            beacon = ON;
+            if(flag == 1){
+                if(base_degree >= 180){
+                   flag = 0;
+                }
+                else {
+                    base_degree += 5;
+                }
+            }
+            else if(flag == 0){
+                if(base_degree <= 0){
+                    flag = 1;
+                }
+                else {
+                    base_degree -= 5;
+                }
+            }
+            servo.SetGoalDegrees( BASE , base_degree-90 , 1);
+        }
+        else if(turning == 0){
+           // turning = 3;
+            beacon = ON;
+            servo.SetGoalDegrees( BASE , base_degree - 90 , 1);
+            pc.printf("base_degree = %d \n", base_degree - 90);
+        }
+        else if(turning == 2){
+            beacon = OFF;
+            servo.SetGoalDegrees( BASE , 0 , 1);
+            base_degree = 90;
+            wait(0.5);
+        }
     }
-}
\ No newline at end of file
+  
+}
+
+   
+
+