GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@7:9aaa4f73bb32, 2016-11-07 (annotated)

Committer:
lh14g13
Date:
Mon Nov 07 16:00:45 2016 +0000
Branch:
testing
Revision:
7:9aaa4f73bb32
Parent:
5:c50e40797114
Child:
8:4df2a47ab4ee
just added in a method to remove the amount of functions;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lh14g13 0:b0476dcfa14c 1 #include "mbed.h"
lh14g13 0:b0476dcfa14c 2 #include "TFC.h"
lh14g13 0:b0476dcfa14c 3 DigitalOut myled(LED1);
lh14g13 0:b0476dcfa14c 4
lh14g13 7:9aaa4f73bb32 5
lh14g13 0:b0476dcfa14c 6 void TurnOn();
lh14g13 7:9aaa4f73bb32 7 void runMotor();
lh14g13 0:b0476dcfa14c 8 void DefaultMode();
lh14g13 0:b0476dcfa14c 9
lh14g13 0:b0476dcfa14c 10
lh14g13 0:b0476dcfa14c 11 //Speed control
lh14g13 0:b0476dcfa14c 12 void Acc(float& motorA, float& motorB);
lh14g13 0:b0476dcfa14c 13 void Decc(float& motorA, float& motorB);
lh14g13 0:b0476dcfa14c 14 void StartLine();
lh14g13 0:b0476dcfa14c 15
lh14g13 0:b0476dcfa14c 16 //Corner Control
lh14g13 0:b0476dcfa14c 17
lh14g13 0:b0476dcfa14c 18 void PWM_cornerCntrl(bool a,float pwmRatio);
lh14g13 0:b0476dcfa14c 19 void cornerLeft(float speed);
lh14g13 0:b0476dcfa14c 20 void cornerRight(float speed);
lh14g13 0:b0476dcfa14c 21
lh14g13 7:9aaa4f73bb32 22 void steering(float center, float theta, int maxspeed);
lh14g13 0:b0476dcfa14c 23
lh14g13 1:09226806dd15 24 int cornerPwmControl;// this sets the cornering percentage ratio. value between 1 and 0.
lh14g13 0:b0476dcfa14c 25 // 100 means both motors run at the same speed. 0 means that one motor turns off.
lh14g13 0:b0476dcfa14c 26
lh14g13 0:b0476dcfa14c 27
lh14g13 0:b0476dcfa14c 28
lh14g13 0:b0476dcfa14c 29 // uncomment for testing motor functions.
lh14g13 0:b0476dcfa14c 30
lh14g13 0:b0476dcfa14c 31
lh14g13 7:9aaa4f73bb32 32
lh14g13 7:9aaa4f73bb32 33 //need a function for calcu;lating the angle
lh14g13 7:9aaa4f73bb32 34 //need a function for converting w1 to delta. or do i?
lh14g13 0:b0476dcfa14c 35
lh14g13 0:b0476dcfa14c 36
lh14g13 0:b0476dcfa14c 37
lh14g13 7:9aaa4f73bb32 38 void runMotor() /// putting it into this mode for some reason makes a bit of a whinning noise (this may simply just be the motor running)
lh14g13 0:b0476dcfa14c 39 {
lh14g13 0:b0476dcfa14c 40
lh14g13 7:9aaa4f73bb32 41
lh14g13 7:9aaa4f73bb32 42 TFC_SetMotorPWM(0.4,0.7);
lh14g13 0:b0476dcfa14c 43
lh14g13 0:b0476dcfa14c 44 while(1)
lh14g13 0:b0476dcfa14c 45 {
lh14g13 0:b0476dcfa14c 46 if(TFC_ReadPushButton(0)>0)
lh14g13 0:b0476dcfa14c 47 {
lh14g13 0:b0476dcfa14c 48 TFC_SetMotorPWM(0.0,0.0);
lh14g13 7:9aaa4f73bb32 49
lh14g13 0:b0476dcfa14c 50 DefaultMode();
lh14g13 0:b0476dcfa14c 51
lh14g13 0:b0476dcfa14c 52 }
lh14g13 0:b0476dcfa14c 53 }
lh14g13 0:b0476dcfa14c 54 return;
lh14g13 0:b0476dcfa14c 55 }
lh14g13 0:b0476dcfa14c 56
lh14g13 0:b0476dcfa14c 57 void DefaultMode()
lh14g13 0:b0476dcfa14c 58 {
lh14g13 7:9aaa4f73bb32 59 TFC_Init();
lh14g13 0:b0476dcfa14c 60 while(1)
lh14g13 0:b0476dcfa14c 61 {
lh14g13 7:9aaa4f73bb32 62 TFC_HBRIDGE_ENABLE;
lh14g13 0:b0476dcfa14c 63 if(TFC_ReadPushButton(1)>0)
lh14g13 0:b0476dcfa14c 64 {
lh14g13 7:9aaa4f73bb32 65 runMotor();
lh14g13 0:b0476dcfa14c 66 }
lh14g13 0:b0476dcfa14c 67
lh14g13 0:b0476dcfa14c 68 else if(TFC_ReadPushButton(0)>0)
lh14g13 0:b0476dcfa14c 69 {
lh14g13 0:b0476dcfa14c 70 //this will be a debug mode
lh14g13 0:b0476dcfa14c 71 }
lh14g13 0:b0476dcfa14c 72
lh14g13 0:b0476dcfa14c 73
lh14g13 0:b0476dcfa14c 74 }
lh14g13 7:9aaa4f73bb32 75 TFC_HBRIDGE_DISABLE;
lh14g13 0:b0476dcfa14c 76 return;
lh14g13 0:b0476dcfa14c 77 }
lh14g13 0:b0476dcfa14c 78
lh14g13 0:b0476dcfa14c 79
lh14g13 0:b0476dcfa14c 80 void Acc(float& motorA, float& motorB)// set up so as to control both motors during acc. Potential use during corners
lh14g13 0:b0476dcfa14c 81 {
lh14g13 0:b0476dcfa14c 82 motorA = motorA + 0.1;
lh14g13 0:b0476dcfa14c 83 motorB = motorB + 0.1;
lh14g13 0:b0476dcfa14c 84 TFC_SetMotorPWM(motorA,motorB);
lh14g13 0:b0476dcfa14c 85
lh14g13 0:b0476dcfa14c 86
lh14g13 0:b0476dcfa14c 87 return ;
lh14g13 0:b0476dcfa14c 88
lh14g13 0:b0476dcfa14c 89 }
lh14g13 0:b0476dcfa14c 90
lh14g13 0:b0476dcfa14c 91 void Decc(float &motorA, float &motorB)
lh14g13 0:b0476dcfa14c 92 {
lh14g13 0:b0476dcfa14c 93 // a good thing to do would be to have a margin for adjustment so that the car cornering can control the acc+dcc much better.
lh14g13 0:b0476dcfa14c 94
lh14g13 0:b0476dcfa14c 95 motorA = motorA - 0.1;
lh14g13 0:b0476dcfa14c 96 motorB = motorB - 0.1;
lh14g13 0:b0476dcfa14c 97
lh14g13 0:b0476dcfa14c 98 TFC_SetMotorPWM(motorA,motorB);
lh14g13 0:b0476dcfa14c 99
lh14g13 0:b0476dcfa14c 100 return ;
lh14g13 0:b0476dcfa14c 101
lh14g13 0:b0476dcfa14c 102 }
lh14g13 0:b0476dcfa14c 103
lh14g13 0:b0476dcfa14c 104
lh14g13 0:b0476dcfa14c 105 void StartLine()
lh14g13 0:b0476dcfa14c 106 {
lh14g13 0:b0476dcfa14c 107 TFC_HBRIDGE_ENABLE;
lh14g13 0:b0476dcfa14c 108 float a=0;
lh14g13 0:b0476dcfa14c 109 float b=0;
lh14g13 0:b0476dcfa14c 110
lh14g13 0:b0476dcfa14c 111 int x=0 ;
lh14g13 0:b0476dcfa14c 112 while(x<5)
lh14g13 0:b0476dcfa14c 113 {
lh14g13 0:b0476dcfa14c 114 Acc(a,b);
lh14g13 0:b0476dcfa14c 115 wait(0.5);
lh14g13 0:b0476dcfa14c 116 x++ ;
lh14g13 0:b0476dcfa14c 117 }
lh14g13 0:b0476dcfa14c 118
lh14g13 0:b0476dcfa14c 119 return ;
lh14g13 0:b0476dcfa14c 120 }
lh14g13 0:b0476dcfa14c 121
lh14g13 0:b0476dcfa14c 122 void finishLine(float pwmA)
lh14g13 0:b0476dcfa14c 123 {
lh14g13 0:b0476dcfa14c 124 float pwmB= pwmA;
lh14g13 0:b0476dcfa14c 125 while(pwmA>0)
lh14g13 0:b0476dcfa14c 126 {
lh14g13 0:b0476dcfa14c 127
lh14g13 0:b0476dcfa14c 128
lh14g13 0:b0476dcfa14c 129 Decc(pwmA,pwmB);
lh14g13 0:b0476dcfa14c 130
lh14g13 0:b0476dcfa14c 131 }
lh14g13 0:b0476dcfa14c 132 return;
lh14g13 0:b0476dcfa14c 133 }
lh14g13 0:b0476dcfa14c 134
lh14g13 0:b0476dcfa14c 135 void PWM_cornerCntrl(bool a,float pwmRatio)
lh14g13 0:b0476dcfa14c 136 {
lh14g13 0:b0476dcfa14c 137 //A is the Right motor B is the left motor.
lh14g13 0:b0476dcfa14c 138 // may need the value of speed to decelerate.
lh14g13 0:b0476dcfa14c 139 if(a==1)//turn left
lh14g13 0:b0476dcfa14c 140 {
lh14g13 0:b0476dcfa14c 141 cornerLeft(pwmRatio);
lh14g13 0:b0476dcfa14c 142 }
lh14g13 0:b0476dcfa14c 143
lh14g13 4:e15ec9052a78 144 else//turn right
lh14g13 0:b0476dcfa14c 145 {
lh14g13 4:e15ec9052a78 146 cornerRight(pwmRatio);
lh14g13 0:b0476dcfa14c 147 }
lh14g13 0:b0476dcfa14c 148 return;
lh14g13 0:b0476dcfa14c 149 }
lh14g13 0:b0476dcfa14c 150
lh14g13 0:b0476dcfa14c 151
lh14g13 0:b0476dcfa14c 152
lh14g13 0:b0476dcfa14c 153
lh14g13 0:b0476dcfa14c 154 void cornerLeft(float speed)
lh14g13 0:b0476dcfa14c 155 {// when cornering left the left motor slows down more than the right hand side
lh14g13 0:b0476dcfa14c 156 // may just replace with ACC and DECC
lh14g13 2:cc8ddc587af7 157
lh14g13 2:cc8ddc587af7 158 // it may be worth doing this off the change in theta. so that it is insesnsitive to the calibration.
lh14g13 4:e15ec9052a78 159 float diff = speed*cornerPwmControl;
lh14g13 4:e15ec9052a78 160 float w1 = speed + diff;
lh14g13 4:e15ec9052a78 161 float w2 = speed-diff;
lh14g13 2:cc8ddc587af7 162 TFC_SetMotorPWM(w2,w1); //temperary values
lh14g13 1:09226806dd15 163
lh14g13 2:cc8ddc587af7 164
lh14g13 2:cc8ddc587af7 165
lh14g13 2:cc8ddc587af7 166
lh14g13 2:cc8ddc587af7 167
lh14g13 2:cc8ddc587af7 168
lh14g13 0:b0476dcfa14c 169
lh14g13 0:b0476dcfa14c 170 return;
lh14g13 0:b0476dcfa14c 171 }
lh14g13 0:b0476dcfa14c 172
lh14g13 0:b0476dcfa14c 173 void cornerRight(float speed)
lh14g13 0:b0476dcfa14c 174 { // may need to put deceleration control within here.
lh14g13 0:b0476dcfa14c 175 // may just replace with ACC and DECC
lh14g13 1:09226806dd15 176 float diff = speed*cornerPwmControl;
lh14g13 4:e15ec9052a78 177 float w1 = speed + diff;
lh14g13 4:e15ec9052a78 178 float w2 = speed-diff;
lh14g13 1:09226806dd15 179 TFC_SetMotorPWM(w1,w2); //temperary values
lh14g13 0:b0476dcfa14c 180
lh14g13 0:b0476dcfa14c 181 return;
lh14g13 0:b0476dcfa14c 182 }
lh14g13 0:b0476dcfa14c 183
lh14g13 0:b0476dcfa14c 184
lh14g13 2:cc8ddc587af7 185 void deltaCornerLeft(float speed,float deltaTheta)
lh14g13 2:cc8ddc587af7 186 {// when cornering left the left motor slows down more than the right hand side
lh14g13 2:cc8ddc587af7 187 // may just replace with ACC and DECC
lh14g13 3:5b5d5af46804 188 float r;
lh14g13 3:5b5d5af46804 189 float d;
lh14g13 3:5b5d5af46804 190 float l;
lh14g13 2:cc8ddc587af7 191 // it may be worth doing this off the change in theta. so that it is insesnsitive to the calibration.
lh14g13 4:e15ec9052a78 192 float diff= ((d*tan(deltaTheta)/(2*l)));
lh14g13 3:5b5d5af46804 193
lh14g13 4:e15ec9052a78 194 float w1 = (speed/r)*(1+diff);
lh14g13 4:e15ec9052a78 195 float w2 = (speed/r)*(1-diff);
lh14g13 3:5b5d5af46804 196
lh14g13 3:5b5d5af46804 197 // when there is a speed sensor tghe conversion will be much more simplistic. this is basically just guessing.
lh14g13 3:5b5d5af46804 198 // need to convert w1 to the duty cycle
lh14g13 2:cc8ddc587af7 199 TFC_SetMotorPWM(w2,w1); //temperary values
lh14g13 2:cc8ddc587af7 200
lh14g13 2:cc8ddc587af7 201
lh14g13 2:cc8ddc587af7 202 return;
lh14g13 2:cc8ddc587af7 203 }
lh14g13 2:cc8ddc587af7 204
lh14g13 7:9aaa4f73bb32 205
lh14g13 7:9aaa4f73bb32 206
lh14g13 5:c50e40797114 207
lh14g13 7:9aaa4f73bb32 208 float centerWheels(float min, float max)
lh14g13 7:9aaa4f73bb32 209 {
lh14g13 7:9aaa4f73bb32 210 float center= (min +max)/2;
lh14g13 7:9aaa4f73bb32 211
lh14g13 7:9aaa4f73bb32 212 return center;
lh14g13 7:9aaa4f73bb32 213 }
lh14g13 7:9aaa4f73bb32 214
lh14g13 7:9aaa4f73bb32 215 void steering(float center, float theta, int maxspeed,float w1, float w2)
lh14g13 7:9aaa4f73bb32 216 {
lh14g13 7:9aaa4f73bb32 217 float deltaTheta = center- theta;
lh14g13 7:9aaa4f73bb32 218 if(deltaTheta <0)
lh14g13 7:9aaa4f73bb32 219 {
lh14g13 7:9aaa4f73bb32 220 // going left?
lh14g13 7:9aaa4f73bb32 221 corner(w1,w2,deltaTheta);
lh14g13 5:c50e40797114 222 }
lh14g13 2:cc8ddc587af7 223
lh14g13 7:9aaa4f73bb32 224 else()
lh14g13 7:9aaa4f73bb32 225 {
lh14g13 7:9aaa4f73bb32 226 //going right?
lh14g13 7:9aaa4f73bb32 227 corner(w2,w1,deltaTheta)
lh14g13 7:9aaa4f73bb32 228
lh14g13 7:9aaa4f73bb32 229 }
lh14g13 2:cc8ddc587af7 230
lh14g13 7:9aaa4f73bb32 231 return;
lh14g13 7:9aaa4f73bb32 232 }