GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@5:c50e40797114, 2016-10-24 (annotated)

Committer:
lh14g13
Date:
Mon Oct 24 14:12:43 2016 +0000
Revision:
5:c50e40797114
Parent:
4:e15ec9052a78
Child:
7:9aaa4f73bb32
Child:
17:28216063e33e
add4ed advanced cornering;

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