Antoine Laurens / Mbed 2 deprecated DEMO3

mechatronics drive

Dependencies:   BNO055_fusion mbed

Fork of DEMO3 by Edwin Cho

drive.cpp@6:59f239c83de4, 2016-03-20 (annotated)

Committer:
alaurens
Date:
Sun Mar 20 20:32:52 2016 +0000
Revision:
6:59f239c83de4
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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alaurens 6:59f239c83de4 1 //
alaurens 6:59f239c83de4 2 // drive.cpp
alaurens 6:59f239c83de4 3 // MOTORDRIVER
alaurens 6:59f239c83de4 4 //
alaurens 6:59f239c83de4 5 // Created by Antoine Laurens on 15.03.16.
alaurens 6:59f239c83de4 6 // Copyright (c) 2016 Antoine Laurens. All rights reserved.
alaurens 6:59f239c83de4 7 //
alaurens 6:59f239c83de4 8
alaurens 6:59f239c83de4 9 #include "drive.h"
alaurens 6:59f239c83de4 10
alaurens 6:59f239c83de4 11
alaurens 6:59f239c83de4 12 void down(LOCALIZE_xya *xya,int stopPoint,float *pwmDuty1,float *pwmDuty2)
alaurens 6:59f239c83de4 13 {
alaurens 6:59f239c83de4 14 driveStraightD(xya,stopPoint,1,pwmDuty1,pwmDuty2);
alaurens 6:59f239c83de4 15 driveStraightD(xya,xya->y-sque_size/2-20, 0,pwmDuty1,pwmDuty2);
alaurens 6:59f239c83de4 16 }
alaurens 6:59f239c83de4 17
alaurens 6:59f239c83de4 18 void driveStraightD(LOCALIZE_xya *xya,int stopPoint,bool dir,float *pwmDuty1,float *pwmDuty2)
alaurens 6:59f239c83de4 19 {
alaurens 6:59f239c83de4 20 //set direction bit so robot goes backward or forwards
alaurens 6:59f239c83de4 21
alaurens 6:59f239c83de4 22 *pwmDuty1=0.1;
alaurens 6:59f239c83de4 23 *pwmDuty2=0.1;
alaurens 6:59f239c83de4 24 if (dir==1)
alaurens 6:59f239c83de4 25 {
alaurens 6:59f239c83de4 26 while (xya->y<stopPoint)
alaurens 6:59f239c83de4 27 {
alaurens 6:59f239c83de4 28 if(abs(xya->y-stopPoint)<15)
alaurens 6:59f239c83de4 29 {
alaurens 6:59f239c83de4 30 //set pwm to k*dist so it slows down
alaurens 6:59f239c83de4 31 }
alaurens 6:59f239c83de4 32 }
alaurens 6:59f239c83de4 33 }
alaurens 6:59f239c83de4 34 else
alaurens 6:59f239c83de4 35 {
alaurens 6:59f239c83de4 36 while (xya->y>stopPoint)
alaurens 6:59f239c83de4 37 {
alaurens 6:59f239c83de4 38 if(abs(xya->y-stopPoint)<15)
alaurens 6:59f239c83de4 39 {
alaurens 6:59f239c83de4 40 //set pwm to k*dist so it slows down
alaurens 6:59f239c83de4 41 }
alaurens 6:59f239c83de4 42 }
alaurens 6:59f239c83de4 43 }
alaurens 6:59f239c83de4 44
alaurens 6:59f239c83de4 45 *pwmDuty1=0;
alaurens 6:59f239c83de4 46 *pwmDuty2=0;
alaurens 6:59f239c83de4 47
alaurens 6:59f239c83de4 48 }
alaurens 6:59f239c83de4 49
alaurens 6:59f239c83de4 50 void driveStraightS(LOCALIZE_xya *xya,int stopPoint,bool dir,float *pwmDuty1,float *pwmDuty2)
alaurens 6:59f239c83de4 51 {
alaurens 6:59f239c83de4 52 //set direction bit so robot goes backward or forwards
alaurens 6:59f239c83de4 53
alaurens 6:59f239c83de4 54 *pwmDuty1=0.1;
alaurens 6:59f239c83de4 55 *pwmDuty2=0.1;
alaurens 6:59f239c83de4 56
alaurens 6:59f239c83de4 57 if (xya->x>win_w/2)
alaurens 6:59f239c83de4 58 {
alaurens 6:59f239c83de4 59 while (xya->x>stopPoint)
alaurens 6:59f239c83de4 60 {
alaurens 6:59f239c83de4 61 if(abs(xya->y-stopPoint)<15)
alaurens 6:59f239c83de4 62 {
alaurens 6:59f239c83de4 63 //set pwm to k*dist so it slows down
alaurens 6:59f239c83de4 64 }
alaurens 6:59f239c83de4 65 }
alaurens 6:59f239c83de4 66 }
alaurens 6:59f239c83de4 67 else
alaurens 6:59f239c83de4 68 {
alaurens 6:59f239c83de4 69 while (xya->x<stopPoint)
alaurens 6:59f239c83de4 70 {
alaurens 6:59f239c83de4 71 if(abs(xya->y-stopPoint)<15)
alaurens 6:59f239c83de4 72 {
alaurens 6:59f239c83de4 73 //set pwm to k*dist so it slows down
alaurens 6:59f239c83de4 74 }
alaurens 6:59f239c83de4 75 }
alaurens 6:59f239c83de4 76 }
alaurens 6:59f239c83de4 77 *pwmDuty1=0;
alaurens 6:59f239c83de4 78 *pwmDuty2=0;
alaurens 6:59f239c83de4 79 }
alaurens 6:59f239c83de4 80
alaurens 6:59f239c83de4 81 void rotate(int stopAngle,LOCALIZE_xya *xya,float *pwmDuty1,float *pwmDuty2)
alaurens 6:59f239c83de4 82 {
alaurens 6:59f239c83de4 83 int currAngle;
alaurens 6:59f239c83de4 84 int initAngle=xya->a;
alaurens 6:59f239c83de4 85 int a=abs(initAngle-stopAngle)<abs(initAngle-stopAngle+359);
alaurens 6:59f239c83de4 86 int b=initAngle<stopAngle;
alaurens 6:59f239c83de4 87
alaurens 6:59f239c83de4 88 switch(a<<1+b)
alaurens 6:59f239c83de4 89 {
alaurens 6:59f239c83de4 90 case 0: //turn right with wrap around
alaurens 6:59f239c83de4 91
alaurens 6:59f239c83de4 92 //robot must turn right set pwm for that at constant rate
alaurens 6:59f239c83de4 93
alaurens 6:59f239c83de4 94 currAngle=xya->a;
alaurens 6:59f239c83de4 95 /* don't put directly xya-> in the if statement because it could
alaurens 6:59f239c83de4 96 change after I modify and before the beginning of the if statement*/
alaurens 6:59f239c83de4 97 while(currAngle<stopAngle+359)
alaurens 6:59f239c83de4 98 {
alaurens 6:59f239c83de4 99 currAngle=xya->a;
alaurens 6:59f239c83de4 100 if (currAngle<stopAngle)
alaurens 6:59f239c83de4 101 {
alaurens 6:59f239c83de4 102 currAngle=currAngle+359;
alaurens 6:59f239c83de4 103 }
alaurens 6:59f239c83de4 104
alaurens 6:59f239c83de4 105 if (abs(currAngle-stopAngle+359)<15)
alaurens 6:59f239c83de4 106 {
alaurens 6:59f239c83de4 107 //set pwm to proprtionat value
alaurens 6:59f239c83de4 108 //k*currAngl so that it slows down
alaurens 6:59f239c83de4 109 }
alaurens 6:59f239c83de4 110 }
alaurens 6:59f239c83de4 111 break;
alaurens 6:59f239c83de4 112
alaurens 6:59f239c83de4 113 case 1://turn left no wrap around
alaurens 6:59f239c83de4 114 //set pwm so the robot starts turning left on both
alaurens 6:59f239c83de4 115 while(xya->a>stopAngle)
alaurens 6:59f239c83de4 116 {
alaurens 6:59f239c83de4 117 if (abs(xya->a-stopAngle)<15)
alaurens 6:59f239c83de4 118 {
alaurens 6:59f239c83de4 119 //set pwm to k*xya->a to slow it down
alaurens 6:59f239c83de4 120 }
alaurens 6:59f239c83de4 121 }
alaurens 6:59f239c83de4 122 break;
alaurens 6:59f239c83de4 123
alaurens 6:59f239c83de4 124 case 2: //left wrap around
alaurens 6:59f239c83de4 125 //set pwm to go left at const rate
alaurens 6:59f239c83de4 126
alaurens 6:59f239c83de4 127 currAngle=xya->a;
alaurens 6:59f239c83de4 128 /* don't put directly xya-> in the if statement because it could
alaurens 6:59f239c83de4 129 change after I modify and before the beginning of the if statement*/
alaurens 6:59f239c83de4 130 while(currAngle+359>stopAngle)
alaurens 6:59f239c83de4 131 {
alaurens 6:59f239c83de4 132 currAngle=xya->a;
alaurens 6:59f239c83de4 133 if (currAngle>stopAngle)
alaurens 6:59f239c83de4 134 {
alaurens 6:59f239c83de4 135 currAngle=currAngle-359;
alaurens 6:59f239c83de4 136 }
alaurens 6:59f239c83de4 137
alaurens 6:59f239c83de4 138 if (abs(currAngle-stopAngle+359)<15)
alaurens 6:59f239c83de4 139 {
alaurens 6:59f239c83de4 140 //set pwm to proprtionat value
alaurens 6:59f239c83de4 141 //k*currAngl so that it slows down
alaurens 6:59f239c83de4 142 }
alaurens 6:59f239c83de4 143 }
alaurens 6:59f239c83de4 144 break;
alaurens 6:59f239c83de4 145
alaurens 6:59f239c83de4 146 case 3: //turn right no wrap around
alaurens 6:59f239c83de4 147 //set pwm so the robot starts turning right on both
alaurens 6:59f239c83de4 148 while(xya->a<stopAngle)
alaurens 6:59f239c83de4 149 {
alaurens 6:59f239c83de4 150 if (abs(xya->a-stopAngle)<15)
alaurens 6:59f239c83de4 151 {
alaurens 6:59f239c83de4 152 //set pwm to k*xya->a to slow it down
alaurens 6:59f239c83de4 153 }
alaurens 6:59f239c83de4 154 }
alaurens 6:59f239c83de4 155 break;
alaurens 6:59f239c83de4 156 }
alaurens 6:59f239c83de4 157 //set pwm =0 so the robot stops
alaurens 6:59f239c83de4 158
alaurens 6:59f239c83de4 159 }