GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@9:22b119eef1de, 2016-11-07 (annotated)

Committer:
lh14g13
Date:
Mon Nov 07 16:37:58 2016 +0000
Branch:
testing
Revision:
9:22b119eef1de
Parent:
8:4df2a47ab4ee
Child:
10:f4fc8ccde4ad
tightened up the code as well as seperating speed change and calculating speed change.;

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 9:22b119eef1de 3 #include <math.h>
lh14g13 0:b0476dcfa14c 4 DigitalOut myled(LED1);
lh14g13 0:b0476dcfa14c 5
lh14g13 7:9aaa4f73bb32 6
lh14g13 0:b0476dcfa14c 7 void TurnOn();
lh14g13 7:9aaa4f73bb32 8 void runMotor();
lh14g13 0:b0476dcfa14c 9 void DefaultMode();
lh14g13 0:b0476dcfa14c 10
lh14g13 0:b0476dcfa14c 11
lh14g13 0:b0476dcfa14c 12 //Speed control
lh14g13 0:b0476dcfa14c 13 void Acc(float& motorA, float& motorB);
lh14g13 0:b0476dcfa14c 14 void Decc(float& motorA, float& motorB);
lh14g13 0:b0476dcfa14c 15 void StartLine();
lh14g13 0:b0476dcfa14c 16
lh14g13 0:b0476dcfa14c 17 //Corner Control
lh14g13 0:b0476dcfa14c 18
lh14g13 8:4df2a47ab4ee 19
lh14g13 8:4df2a47ab4ee 20 void corner(float &w1,float &w2,float deltaTheta,int maxspeed)
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
lh14g13 9:22b119eef1de 136 void speedSetting(int w1, int w2 ,int w1M, int w2M)
lh14g13 9:22b119eef1de 137 {
lh14g13 9:22b119eef1de 138 // need to compare the measured frequency
lh14g13 9:22b119eef1de 139
lh14g13 9:22b119eef1de 140 float deltaW1 = w1 - w1M;
lh14g13 9:22b119eef1de 141 float deltaW2 = w2 - w2M;
lh14g13 9:22b119eef1de 142
lh14g13 9:22b119eef1de 143 if(delatW1 <0)
lh14g13 9:22b119eef1de 144 {
lh14g13 9:22b119eef1de 145 changespeed(0,w1);
lh14g13 9:22b119eef1de 146 }
lh14g13 9:22b119eef1de 147
lh14g13 9:22b119eef1de 148 else if(delatW1 >0)
lh14g13 9:22b119eef1de 149 {
lh14g13 9:22b119eef1de 150 changespeed(1,w1);
lh14g13 9:22b119eef1de 151 }
lh14g13 9:22b119eef1de 152 return;
lh14g13 9:22b119eef1de 153 }
lh14g13 9:22b119eef1de 154
lh14g13 9:22b119eef1de 155 //need to fill out function for calculating the change in speed.
lh14g13 9:22b119eef1de 156 void changespeed(bool a, float w)
lh14g13 9:22b119eef1de 157 {
lh14g13 9:22b119eef1de 158 float change;
lh14g13 9:22b119eef1de 159
lh14g13 9:22b119eef1de 160 if(a == 1)
lh14g13 9:22b119eef1de 161 {
lh14g13 9:22b119eef1de 162 w+= change;
lh14g13 9:22b119eef1de 163 }
lh14g13 9:22b119eef1de 164
lh14g13 9:22b119eef1de 165 else if (a ==0)
lh14g13 9:22b119eef1de 166 {
lh14g13 9:22b119eef1de 167
lh14g13 9:22b119eef1de 168 w-= change;
lh14g13 9:22b119eef1de 169 }
lh14g13 9:22b119eef1de 170 return;
lh14g13 9:22b119eef1de 171 }
lh14g13 9:22b119eef1de 172
lh14g13 8:4df2a47ab4ee 173 //----------------------------------------------------------------------------------------------------------------------------
lh14g13 8:4df2a47ab4ee 174 //------------------------------------------------Cornering Control-----------------------------------------------------------
lh14g13 8:4df2a47ab4ee 175 //----------------------------------------------------------------------------------------------------------------------------
lh14g13 2:cc8ddc587af7 176
lh14g13 2:cc8ddc587af7 177
lh14g13 8:4df2a47ab4ee 178 void corner(float &w1,float &w2,float deltaTheta,int maxspeed)
lh14g13 2:cc8ddc587af7 179 {// when cornering left the left motor slows down more than the right hand side
lh14g13 2:cc8ddc587af7 180 // may just replace with ACC and DECC
lh14g13 3:5b5d5af46804 181 float r;
lh14g13 3:5b5d5af46804 182 float d;
lh14g13 3:5b5d5af46804 183 float l;
lh14g13 2:cc8ddc587af7 184 // it may be worth doing this off the change in theta. so that it is insesnsitive to the calibration.
lh14g13 4:e15ec9052a78 185 float diff= ((d*tan(deltaTheta)/(2*l)));
lh14g13 3:5b5d5af46804 186
lh14g13 8:4df2a47ab4ee 187 float w1 = (maxspeed/r)*(1+diff);
lh14g13 8:4df2a47ab4ee 188 float w2 = (maxspeed/r)*(1-diff);
lh14g13 3:5b5d5af46804 189
lh14g13 8:4df2a47ab4ee 190
lh14g13 2:cc8ddc587af7 191
lh14g13 2:cc8ddc587af7 192
lh14g13 2:cc8ddc587af7 193 return;
lh14g13 2:cc8ddc587af7 194 }
lh14g13 2:cc8ddc587af7 195
lh14g13 7:9aaa4f73bb32 196
lh14g13 7:9aaa4f73bb32 197
lh14g13 5:c50e40797114 198
lh14g13 7:9aaa4f73bb32 199 float centerWheels(float min, float max)
lh14g13 7:9aaa4f73bb32 200 {
lh14g13 7:9aaa4f73bb32 201 float center= (min +max)/2;
lh14g13 7:9aaa4f73bb32 202
lh14g13 7:9aaa4f73bb32 203 return center;
lh14g13 7:9aaa4f73bb32 204 }
lh14g13 7:9aaa4f73bb32 205
lh14g13 8:4df2a47ab4ee 206 void steering(float center, float theta, int maxspeed,float & w1, float & w2)
lh14g13 8:4df2a47ab4ee 207 { // this function is for simply calculating the motor speeds. this reduces the amount of calculations needed
lh14g13 8:4df2a47ab4ee 208 //and can be triggered when the car steers.
lh14g13 8:4df2a47ab4ee 209
lh14g13 8:4df2a47ab4ee 210
lh14g13 7:9aaa4f73bb32 211 float deltaTheta = center- theta;
lh14g13 9:22b119eef1de 212
lh14g13 9:22b119eef1de 213 // need to convert to degrees/radians. that can do the above as well.
lh14g13 7:9aaa4f73bb32 214 if(deltaTheta <0)
lh14g13 7:9aaa4f73bb32 215 {
lh14g13 7:9aaa4f73bb32 216 // going left?
lh14g13 8:4df2a47ab4ee 217 corner(w1,w2,deltaTheta,maxspeed);
lh14g13 5:c50e40797114 218 }
lh14g13 2:cc8ddc587af7 219
lh14g13 7:9aaa4f73bb32 220 else()
lh14g13 7:9aaa4f73bb32 221 {
lh14g13 7:9aaa4f73bb32 222 //going right?
lh14g13 9:22b119eef1de 223 corner(w2,w1,deltaTheta);
lh14g13 7:9aaa4f73bb32 224
lh14g13 7:9aaa4f73bb32 225 }
lh14g13 2:cc8ddc587af7 226
lh14g13 7:9aaa4f73bb32 227 return;
lh14g13 7:9aaa4f73bb32 228 }