mechatronics drive
Dependencies: BNO055_fusion mbed
Fork of DEMO3 by
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