Signa-bot code for project BioRobotics, at University of Twente.

Dependencies:   mbed QEI MODSERIAL FastPWM ttmath Math

Files at this revision

API Documentation at this revision

Comitter:
viviien
Date:
Sat Nov 02 14:48:42 2019 +0000
Parent:
35:4cb2ed6dd2d2
Commit message:
Versie voor tijdens de demo (cirkel)

Changed in this revision

Motor_tryout.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 4cb2ed6dd2d2 -r 15446e49ff3d Motor_tryout.cpp
--- a/Motor_tryout.cpp	Fri Nov 01 08:24:42 2019 +0000
+++ b/Motor_tryout.cpp	Sat Nov 02 14:48:42 2019 +0000
@@ -218,11 +218,9 @@
     
 //z0 = 158 - 60* sin(3*time_sin); 
 
-if  (sign == 1) {
     float r = 20*(1-1/(1+(time_sin)));
-    x0 = savedX0 - r*sin(3*time_sin);
-    y0 = savedY0 - r*cos(3*time_sin);
-}
+    x0 =  r*sin(3*time_sin);
+    y0 =  r*cos(3*time_sin);
      
 
     delta_calctheta1 ();  
@@ -294,8 +292,6 @@
     delta_calctheta3 ();
     
     pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-
-    char cc = pc.getc();
     
         motor_timer.attach(&motor, 0.002);
 
@@ -308,6 +304,9 @@
             pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
             pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
             pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+            
+                char cc = pc.getc();
+
 
     
     
@@ -315,83 +314,83 @@
 
     while(true){
         
-        while (boxcheckC == 0)  {    
-        if (boxcheckA == 1 && x0>=-76 && x0<=75) {
-            x0=x0+1.0f ;
-            wait(1.0/20);
-            }
-        if (boxcheckB== 1 && x0>=-75 && x0<=76) {            
-            x0=x0-1.0f;
-            wait(1.0/20);
-            }
-        }
-        
-        wait(1.5);
-        savedX = x0;
-        
-        while (boxcheckC == 0)  {
-        if (boxcheckA==1 && y0>=-76 && y0<=75){
-            y0=y0+1.0f;
-            wait(1.0/20);
-            }
-        if (boxcheckB==1 && y0>=-75 && y0<=76){
-            y0=y0-1.0f;
-            wait(1.0/20);
-            }
-        }
-        
-        wait(1.5);
-        savedY = y0;
-        
-        while (boxcheckC == 0)  {
-        if (boxcheckA == 1 && z0>=-179 && z0<=-158){
-            z0=z0+1.0f;
-            wait(1.0/20);
-            }
-        if (boxcheckB == 1 && z0>=-178 && z0<=-157){
-            z0=z0-1.0f;
-            wait(1.0/20);
-            }
-        }
-        
-        wait(1.5);
-        savedZ = z0;
-        
-        if (boxcheckC == 1)     {
-            sign = 1;
-            }
-        
-        wait(5.0);
-        
-        sign = 0;
-        
-        float diffZ = -158 - z0;
-        float diffX = 0 - x0;
-        float diffY = 0 - y0;
-        
-        for (int i=0; i<=diffZ; i++)    {
-         z0 = z0+1;
-         }
-         
-        for (int i=0; i<=diffY; i++)    {
-         if (diffY>0)   {
-            y0 = y0+1;
-            }
-         if (diffY<0)   {
-             y0 = y0-1;
-             }
-         }
-         
-        for (int i=0; i<=diffX; i++)    {
-         if (diffX>0)   {
-            x0 = x0+1;
-            }
-         if (diffX<0)   {
-             x0 = x0-1;
-             }
-         }
-
-break;        
+//        while (boxcheckC == 0)  {    
+//        if (boxcheckA == 1 && x0>=-76 && x0<=75) {
+//            x0=x0+1.0f ;
+//            wait(1.0/20);
+//            }
+//        if (boxcheckB== 1 && x0>=-75 && x0<=76) {            
+//            x0=x0-1.0f;
+//            wait(1.0/20);
+//            }
+//        }
+//        
+//        wait(1.5);
+//        savedX = x0;
+//        
+//        while (boxcheckC == 0)  {
+//        if (boxcheckA==1 && y0>=-76 && y0<=75){
+//            y0=y0+1.0f;
+//            wait(1.0/20);
+//            }
+//        if (boxcheckB==1 && y0>=-75 && y0<=76){
+//            y0=y0-1.0f;
+//            wait(1.0/20);
+//            }
+//        }
+//        
+//        wait(1.5);
+//        savedY = y0;
+//        
+//        while (boxcheckC == 0)  {
+//        if (boxcheckA == 1 && z0>=-179 && z0<=-158){
+//            z0=z0+1.0f;
+//            wait(1.0/20);
+//            }
+//        if (boxcheckB == 1 && z0>=-178 && z0<=-157){
+//            z0=z0-1.0f;
+//            wait(1.0/20);
+//            }
+//        }
+//        
+//        wait(1.5);
+//        savedZ = z0;
+//        
+//        if (boxcheckC == 1)     {
+//            sign = 1;
+//            }
+//        
+//        wait(5.0);
+//        
+//        sign = 0;
+//        
+//        float diffZ = -158 - z0;
+//        float diffX = 0 - x0;
+//        float diffY = 0 - y0;
+//        
+//        for (int i=0; i<=diffZ; i++)    {
+//         z0 = z0+1;
+//         }
+//         
+//        for (int i=0; i<=diffY; i++)    {
+//         if (diffY>0)   {
+//            y0 = y0+1;
+//            }
+//         if (diffY<0)   {
+//             y0 = y0-1;
+//             }
+//         }
+//         
+//        for (int i=0; i<=diffX; i++)    {
+//         if (diffX>0)   {
+//            x0 = x0+1;
+//            }
+//         if (diffX<0)   {
+//             x0 = x0-1;
+//             }
+//         }
+//
+//break;        
 
             
         
@@ -400,44 +399,44 @@
 //        wait(0.1);
 
        
-//       char cc = pc.getc();
-//        if (cc=='d' && x0>=-76 && x0<=75) {
-//            x0=x0+1.0f ;
-//            }
-//
-//        if (cc=='a' && x0>=-75 && x0<=76) {
-//            
-//            x0=x0-1.0f;
-//            }
-//
-//        if (cc=='w' && y0>=-76 && y0<=75){
-//            y0=y0+1.0f;
-//            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-//            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
-//            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
-//
-//            }
-//
-//        if (cc=='s' && y0>=-75 && y0<=76){
-//            y0=y0-1.0f;
-//            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-//            pc  .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
-//            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
-//            }
-//
-//        if (cc=='u' && z0>=-211 && z0<=-130){
-//            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-//            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
-//            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
-//            z0=z0+1.0f;
-//            }
-//
-//        if (cc=='j' && z0>=-210 && z0<=-129){
-//                        pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-//            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
-//            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
-//            z0=z0-1.0f;
-//            }
+       char cc = pc.getc();
+        if (cc=='d' && x0>=-76 && x0<=75) {
+            x0=x0+1.0f ;
+            }
+
+        if (cc=='a' && x0>=-75 && x0<=76) {
+            
+            x0=x0-1.0f;
+            }
+
+        if (cc=='w' && y0>=-76 && y0<=75){
+            y0=y0+1.0f;
+            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
+            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
+            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+
+            }
+
+        if (cc=='s' && y0>=-75 && y0<=76){
+            y0=y0-1.0f;
+            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
+            pc  .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
+            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+            }
+
+        if (cc=='u' && z0>=-211 && z0<=-130){
+            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
+            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
+            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+            z0=z0+1.0f;
+            }
+
+        if (cc=='j' && z0>=-210 && z0<=-129){
+                        pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
+            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
+            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+            z0=z0-1.0f;
+            }
         }
 //END MAIN
 }  
\ No newline at end of file