Antoine Laurens
mechatronics drive
Dependencies: BNO055_fusion mbed
Revision 6:59f239c83de4, committed 2016-03-20
- 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
--- /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
--- /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__) */
--- /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
--- 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