Lawrence Harlow / motor

changes to motor library

Fork of motor by GDP 4

motor.cpp@3:5b5d5af46804, 2016-10-21 (annotated)

Committer:
lh14g13
Date:
Fri Oct 21 13:41:03 2016 +0000
Revision:
3:5b5d5af46804
Parent:
2:cc8ddc587af7
Child:
4:e15ec9052a78
adding in a need for the sensitivity control

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 0:b0476dcfa14c 146 else()//turn right
lh14g13 0:b0476dcfa14c 147 {
lh14g13 0:b0476dcfa14c 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 1:09226806dd15 161 w1 = speed + diff;
lh14g13 2:cc8ddc587af7 162 w2 = speed-diff;
lh14g13 2:cc8ddc587af7 163 TFC_SetMotorPWM(w2,w1); //temperary values
lh14g13 1:09226806dd15 164
lh14g13 2:cc8ddc587af7 165
lh14g13 2:cc8ddc587af7 166
lh14g13 2:cc8ddc587af7 167
lh14g13 2:cc8ddc587af7 168
lh14g13 2:cc8ddc587af7 169
lh14g13 0:b0476dcfa14c 170
lh14g13 0:b0476dcfa14c 171 return;
lh14g13 0:b0476dcfa14c 172 }
lh14g13 0:b0476dcfa14c 173
lh14g13 0:b0476dcfa14c 174 void cornerRight(float speed)
lh14g13 0:b0476dcfa14c 175 { // may need to put deceleration control within here.
lh14g13 0:b0476dcfa14c 176 // may just replace with ACC and DECC
lh14g13 1:09226806dd15 177 float diff = speed*cornerPwmControl;
lh14g13 1:09226806dd15 178 w1 = speed + diff;
lh14g13 1:09226806dd15 179 w2 = speed-diff;
lh14g13 1:09226806dd15 180 TFC_SetMotorPWM(w1,w2); //temperary values
lh14g13 0:b0476dcfa14c 181
lh14g13 0:b0476dcfa14c 182 return;
lh14g13 0:b0476dcfa14c 183 }
lh14g13 0:b0476dcfa14c 184
lh14g13 0:b0476dcfa14c 185
lh14g13 2:cc8ddc587af7 186 void deltaCornerLeft(float speed,float deltaTheta)
lh14g13 2:cc8ddc587af7 187 {// when cornering left the left motor slows down more than the right hand side
lh14g13 2:cc8ddc587af7 188 // may just replace with ACC and DECC
lh14g13 3:5b5d5af46804 189 float r;
lh14g13 3:5b5d5af46804 190 float d;
lh14g13 3:5b5d5af46804 191 float l;
lh14g13 2:cc8ddc587af7 192 // it may be worth doing this off the change in theta. so that it is insesnsitive to the calibration.
lh14g13 3:5b5d5af46804 193 diff= ((d*tan(deltatheta)/(2*l)));
lh14g13 3:5b5d5af46804 194
lh14g13 3:5b5d5af46804 195 w1 = (speed/r)*(1+diff);
lh14g13 3:5b5d5af46804 196 w2 = (speed/r)*(1-diff);
lh14g13 3:5b5d5af46804 197
lh14g13 3:5b5d5af46804 198 // when there is a speed sensor tghe conversion will be much more simplistic. this is basically just guessing.
lh14g13 3:5b5d5af46804 199 // need to convert w1 to the duty cycle
lh14g13 2:cc8ddc587af7 200 TFC_SetMotorPWM(w2,w1); //temperary values
lh14g13 2:cc8ddc587af7 201
lh14g13 2:cc8ddc587af7 202
lh14g13 2:cc8ddc587af7 203 return;
lh14g13 2:cc8ddc587af7 204 }
lh14g13 2:cc8ddc587af7 205
lh14g13 2:cc8ddc587af7 206
lh14g13 2:cc8ddc587af7 207