mechatronics drive

Dependencies:   BNO055_fusion mbed

Fork of DEMO3 by Edwin Cho

Files at this revision

API Documentation at this revision

Comitter:
alaurens
Date:
Sun Mar 20 20:32:52 2016 +0000
Parent:
5:c89308dc1827
Commit message:
added 3 files drive.h drive.cpp and main.h mains contains constants defined for my other function.; drive.cpp contains 3 function drivestraightS(sideways) driveStraightD(down) and rotate

Changed in this revision

drive.cpp Show annotated file Show diff for this revision Revisions of this file
drive.h Show annotated file Show diff for this revision Revisions of this file
main.h Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r c89308dc1827 -r 59f239c83de4 drive.cpp
--- /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
diff -r c89308dc1827 -r 59f239c83de4 drive.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/drive.h	Sun Mar 20 20:32:52 2016 +0000
@@ -0,0 +1,29 @@
+//
+//  drive.h
+//  MOTORDRIVER
+//
+//  Created by Antoine Laurens on 15.03.16.
+//  Copyright (c) 2016 Antoine Laurens. All rights reserved.
+//
+
+#ifndef __MOTORDRIVER__drive__
+#define __MOTORDRIVER__drive__
+
+#include <stdio.h>
+#include "main.h"
+#include "LOCALIZE.h"
+void down(LOCALIZE_xya *xya,int stopPoint,float *pwmDuty1,float *pwmDuty2);                     //when the robot goes downwards
+
+void driveStraightD(LOCALIZE_xya *xya,int stopPoint,bool dir,float *pwmDuty1,float *pwmDuty2);     //when the robot drives straight (forward or backwards)
+void driveStraightS(LOCALIZE_xya *xya,int stopPoint,bool dir,float *pwmDuty1,float *pwmDuty2);     //D down S Side
+
+void rotate(int stopAngle,int initAngle,int robPosx,float *pwmDuty1,float *pwmDuty2); //when the robot rotates use *xya instead of init angle and robPosx
+
+void crossSep();
+
+void feedbackControl(LOCALIZE_xya *xya,int line,int angle,float *pwmDuty1,float *pwmDuty2);
+
+
+    
+
+#endif /* defined(__MOTORDRIVER__drive__) */
diff -r c89308dc1827 -r 59f239c83de4 main.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Sun Mar 20 20:32:52 2016 +0000
@@ -0,0 +1,23 @@
+//
+//  main.h
+//  MOTORDRIVER
+//
+//  Created by Antoine Laurens on 15.03.16.
+//  Copyright (c) 2016 Antoine Laurens. All rights reserved.
+//
+
+#ifndef MOTORDRIVER_main_h
+#define MOTORDRIVER_main_h
+#include "mbed.h"
+
+#define sque_size 300        //squeegee size
+#define rob_w 200            //robot width
+#define rob_l 200            //robot length
+#define win_h 1800           //window height
+#define win_w 1500           //window width
+#define overlap 30          //overlap with the squeegee on each passage
+#define sep_t   40          // separator thickness
+#define sep_h   22          // separator height
+
+
+#endif
diff -r c89308dc1827 -r 59f239c83de4 mbed.bld
--- a/mbed.bld	Sun Mar 06 18:31:59 2016 +0000
+++ b/mbed.bld	Sun Mar 20 20:32:52 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/87f2f5183dfb
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/c0f6e94411f5
\ No newline at end of file