Diff: drive.cpp

Revision:

6:59f239c83de4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/drive.cpp	Sun Mar 20 20:32:52 2016 +0000
@@ -0,0 +1,159 @@
+//
+//  drive.cpp
+//  MOTORDRIVER
+//
+//  Created by Antoine Laurens on 15.03.16.
+//  Copyright (c) 2016 Antoine Laurens. All rights reserved.
+//
+
+#include "drive.h"
+
+
+void down(LOCALIZE_xya *xya,int stopPoint,float *pwmDuty1,float *pwmDuty2)
+{
+    driveStraightD(xya,stopPoint,1,pwmDuty1,pwmDuty2);
+    driveStraightD(xya,xya->y-sque_size/2-20, 0,pwmDuty1,pwmDuty2);
+}
+
+void driveStraightD(LOCALIZE_xya *xya,int stopPoint,bool dir,float *pwmDuty1,float *pwmDuty2)
+{
+    //set direction bit so robot goes backward or forwards
+    
+    *pwmDuty1=0.1;
+    *pwmDuty2=0.1;    
+    if (dir==1)
+    {
+        while (xya->y<stopPoint)
+        { 
+            if(abs(xya->y-stopPoint)<15)
+            {
+                //set pwm to k*dist so it slows down
+            }          
+        }
+    }
+    else 
+    {
+        while (xya->y>stopPoint)
+        {    
+            if(abs(xya->y-stopPoint)<15)
+            {
+                //set pwm to k*dist so it slows down
+            }      
+        }
+    }
+    
+    *pwmDuty1=0;
+    *pwmDuty2=0;
+ 
+}
+
+void driveStraightS(LOCALIZE_xya *xya,int stopPoint,bool dir,float *pwmDuty1,float *pwmDuty2)
+{
+    //set direction bit so robot goes backward or forwards
+    
+    *pwmDuty1=0.1;
+    *pwmDuty2=0.1;
+    
+    if (xya->x>win_w/2)
+    {
+        while (xya->x>stopPoint)
+        {
+            if(abs(xya->y-stopPoint)<15)
+            {
+                //set pwm to k*dist so it slows down
+            }          
+        }
+    } 
+    else
+    {
+        while (xya->x<stopPoint)
+        {
+            if(abs(xya->y-stopPoint)<15)
+            {
+                //set pwm to k*dist so it slows down
+            }          
+        }
+    }   
+    *pwmDuty1=0;
+    *pwmDuty2=0; 
+}
+
+void rotate(int stopAngle,LOCALIZE_xya *xya,float *pwmDuty1,float *pwmDuty2)
+{
+    int currAngle;
+    int initAngle=xya->a;
+    int a=abs(initAngle-stopAngle)<abs(initAngle-stopAngle+359);
+    int b=initAngle<stopAngle;
+
+    switch(a<<1+b)
+    {
+        case 0:  //turn right with wrap around
+        
+            //robot must turn right set pwm for that at constant rate
+            
+            currAngle=xya->a;
+            /* don't put directly xya-> in the if statement because it could 
+            change after I modify and before the beginning of the if statement*/
+            while(currAngle<stopAngle+359)
+            {
+                currAngle=xya->a;        
+                if (currAngle<stopAngle)
+                {
+                    currAngle=currAngle+359;
+                }
+                
+                if (abs(currAngle-stopAngle+359)<15)
+                {
+                    //set pwm to proprtionat value 
+                    //k*currAngl so that it slows down 
+                }
+            }
+        break;
+
+        case 1://turn left no wrap around
+            //set pwm so the robot starts turning left on both
+            while(xya->a>stopAngle)
+            {
+                if (abs(xya->a-stopAngle)<15)
+                {
+                    //set pwm to k*xya->a to slow it down
+                }    
+            }    
+        break;
+        
+        case 2: //left wrap around
+            //set pwm to go left at const rate
+        
+            currAngle=xya->a;
+            /* don't put directly xya-> in the if statement because it could 
+            change after I modify and before the beginning of the if statement*/
+            while(currAngle+359>stopAngle)
+            {
+                currAngle=xya->a;        
+                if (currAngle>stopAngle)
+                {
+                    currAngle=currAngle-359;
+                }
+                
+                if (abs(currAngle-stopAngle+359)<15)
+                {
+                    //set pwm to proprtionat value 
+                    //k*currAngl so that it slows down 
+                }
+            }
+        break;
+
+        case 3: //turn right no wrap around
+            //set pwm so the robot starts turning right on both
+            while(xya->a<stopAngle)
+            {
+                if (abs(xya->a-stopAngle)<15)
+                {
+                    //set pwm to k*xya->a to slow it down
+                }    
+            }    
+        break;
+    }
+    //set pwm =0 so the robot stops
+        
+}
\ No newline at end of file