GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@14:bc77edc4adb0, 2016-11-20 (annotated)

Committer:
lh14g13
Date:
Sun Nov 20 13:24:43 2016 +0000
Branch:
motorupdate
Revision:
14:bc77edc4adb0
Parent:
13:c43f157a6bac
Child:
15:f40e834d063b
added speedadj() function;

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 11:4a6f97cc1f1e 19 void dutyCycleCorner( float speed, float angle);
lh14g13 11:4a6f97cc1f1e 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 11:4a6f97cc1f1e 23 void centerWheels();
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 10:f4fc8ccde4ad 36 //----------------------------------------------------------------------------------------------------------------------------------
lh14g13 10:f4fc8ccde4ad 37 //----------------------------------------------This is for Motor Set up------------------------------------------------------------
lh14g13 10:f4fc8ccde4ad 38 //----------------------------------------------------------------------------------------------------------------------------------
lh14g13 0:b0476dcfa14c 39
lh14g13 7:9aaa4f73bb32 40 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 41 {
lh14g13 0:b0476dcfa14c 42
lh14g13 7:9aaa4f73bb32 43
lh14g13 7:9aaa4f73bb32 44 TFC_SetMotorPWM(0.4,0.7);
lh14g13 0:b0476dcfa14c 45
lh14g13 0:b0476dcfa14c 46 while(1)
lh14g13 0:b0476dcfa14c 47 {
lh14g13 0:b0476dcfa14c 48 if(TFC_ReadPushButton(0)>0)
lh14g13 0:b0476dcfa14c 49 {
lh14g13 0:b0476dcfa14c 50 TFC_SetMotorPWM(0.0,0.0);
lh14g13 7:9aaa4f73bb32 51
lh14g13 0:b0476dcfa14c 52 DefaultMode();
lh14g13 0:b0476dcfa14c 53
lh14g13 0:b0476dcfa14c 54 }
lh14g13 0:b0476dcfa14c 55 }
lh14g13 0:b0476dcfa14c 56 return;
lh14g13 0:b0476dcfa14c 57 }
lh14g13 0:b0476dcfa14c 58
lh14g13 0:b0476dcfa14c 59 void DefaultMode()
lh14g13 0:b0476dcfa14c 60 {
lh14g13 7:9aaa4f73bb32 61 TFC_Init();
lh14g13 0:b0476dcfa14c 62 while(1)
lh14g13 0:b0476dcfa14c 63 {
lh14g13 7:9aaa4f73bb32 64 TFC_HBRIDGE_ENABLE;
lh14g13 0:b0476dcfa14c 65 if(TFC_ReadPushButton(1)>0)
lh14g13 0:b0476dcfa14c 66 {
lh14g13 7:9aaa4f73bb32 67 runMotor();
lh14g13 0:b0476dcfa14c 68 }
lh14g13 0:b0476dcfa14c 69
lh14g13 0:b0476dcfa14c 70 else if(TFC_ReadPushButton(0)>0)
lh14g13 0:b0476dcfa14c 71 {
lh14g13 0:b0476dcfa14c 72 //this will be a debug mode
lh14g13 0:b0476dcfa14c 73 }
lh14g13 0:b0476dcfa14c 74
lh14g13 0:b0476dcfa14c 75
lh14g13 0:b0476dcfa14c 76 }
lh14g13 7:9aaa4f73bb32 77 TFC_HBRIDGE_DISABLE;
lh14g13 0:b0476dcfa14c 78 return;
lh14g13 0:b0476dcfa14c 79 }
lh14g13 10:f4fc8ccde4ad 80 //-----------------------------------------------------------------------------------------------------
lh14g13 10:f4fc8ccde4ad 81 //------------------------ this is for speed control---------------------------------------------------
lh14g13 10:f4fc8ccde4ad 82 //-----------------------------------------------------------------------------------------------------
lh14g13 10:f4fc8ccde4ad 83
lh14g13 10:f4fc8ccde4ad 84
lh14g13 0:b0476dcfa14c 85
lh14g13 0:b0476dcfa14c 86 void Acc(float& motorA, float& motorB)// set up so as to control both motors during acc. Potential use during corners
lh14g13 0:b0476dcfa14c 87 {
lh14g13 0:b0476dcfa14c 88 motorA = motorA + 0.1;
lh14g13 0:b0476dcfa14c 89 motorB = motorB + 0.1;
lh14g13 0:b0476dcfa14c 90 TFC_SetMotorPWM(motorA,motorB);
lh14g13 0:b0476dcfa14c 91
lh14g13 0:b0476dcfa14c 92
lh14g13 0:b0476dcfa14c 93 return ;
lh14g13 0:b0476dcfa14c 94
lh14g13 0:b0476dcfa14c 95 }
lh14g13 0:b0476dcfa14c 96
lh14g13 0:b0476dcfa14c 97 void Decc(float &motorA, float &motorB)
lh14g13 0:b0476dcfa14c 98 {
lh14g13 0:b0476dcfa14c 99 // 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 100
lh14g13 0:b0476dcfa14c 101 motorA = motorA - 0.1;
lh14g13 0:b0476dcfa14c 102 motorB = motorB - 0.1;
lh14g13 0:b0476dcfa14c 103
lh14g13 0:b0476dcfa14c 104 TFC_SetMotorPWM(motorA,motorB);
lh14g13 0:b0476dcfa14c 105
lh14g13 0:b0476dcfa14c 106 return ;
lh14g13 0:b0476dcfa14c 107
lh14g13 0:b0476dcfa14c 108 }
lh14g13 0:b0476dcfa14c 109
lh14g13 0:b0476dcfa14c 110
lh14g13 0:b0476dcfa14c 111 void StartLine()
lh14g13 0:b0476dcfa14c 112 {
lh14g13 0:b0476dcfa14c 113 TFC_HBRIDGE_ENABLE;
lh14g13 0:b0476dcfa14c 114 float a=0;
lh14g13 0:b0476dcfa14c 115 float b=0;
lh14g13 0:b0476dcfa14c 116
lh14g13 0:b0476dcfa14c 117 int x=0 ;
lh14g13 0:b0476dcfa14c 118 while(x<5)
lh14g13 0:b0476dcfa14c 119 {
lh14g13 0:b0476dcfa14c 120 Acc(a,b);
lh14g13 0:b0476dcfa14c 121 wait(0.5);
lh14g13 0:b0476dcfa14c 122 x++ ;
lh14g13 0:b0476dcfa14c 123 }
lh14g13 0:b0476dcfa14c 124
lh14g13 0:b0476dcfa14c 125 return ;
lh14g13 0:b0476dcfa14c 126 }
lh14g13 0:b0476dcfa14c 127
lh14g13 0:b0476dcfa14c 128 void finishLine(float pwmA)
lh14g13 0:b0476dcfa14c 129 {
lh14g13 0:b0476dcfa14c 130 float pwmB= pwmA;
lh14g13 0:b0476dcfa14c 131 while(pwmA>0)
lh14g13 0:b0476dcfa14c 132 {
lh14g13 0:b0476dcfa14c 133
lh14g13 0:b0476dcfa14c 134
lh14g13 0:b0476dcfa14c 135 Decc(pwmA,pwmB);
lh14g13 0:b0476dcfa14c 136
lh14g13 0:b0476dcfa14c 137 }
lh14g13 0:b0476dcfa14c 138 return;
lh14g13 0:b0476dcfa14c 139 }
lh14g13 0:b0476dcfa14c 140
lh14g13 0:b0476dcfa14c 141
lh14g13 11:4a6f97cc1f1e 142 /*void speedSetting(int w1, int w2 ,int w1M, int w2M)
lh14g13 9:22b119eef1de 143 {
lh14g13 9:22b119eef1de 144 // need to compare the measured frequency
lh14g13 9:22b119eef1de 145
lh14g13 9:22b119eef1de 146 float deltaW1 = w1 - w1M;
lh14g13 9:22b119eef1de 147 float deltaW2 = w2 - w2M;
lh14g13 9:22b119eef1de 148
lh14g13 11:4a6f97cc1f1e 149 if(deltaW1 <0)
lh14g13 9:22b119eef1de 150 {
lh14g13 9:22b119eef1de 151 changespeed(0,w1);
lh14g13 9:22b119eef1de 152 }
lh14g13 9:22b119eef1de 153
lh14g13 9:22b119eef1de 154 else if(delatW1 >0)
lh14g13 9:22b119eef1de 155 {
lh14g13 9:22b119eef1de 156 changespeed(1,w1);
lh14g13 9:22b119eef1de 157 }
lh14g13 9:22b119eef1de 158 return;
lh14g13 11:4a6f97cc1f1e 159 }*/
lh14g13 9:22b119eef1de 160
lh14g13 9:22b119eef1de 161 //need to fill out function for calculating the change in speed.
lh14g13 9:22b119eef1de 162 void changespeed(bool a, float w)
lh14g13 9:22b119eef1de 163 {
lh14g13 9:22b119eef1de 164 float change;
lh14g13 9:22b119eef1de 165
lh14g13 9:22b119eef1de 166 if(a == 1)
lh14g13 9:22b119eef1de 167 {
lh14g13 9:22b119eef1de 168 w+= change;
lh14g13 9:22b119eef1de 169 }
lh14g13 9:22b119eef1de 170
lh14g13 9:22b119eef1de 171 else if (a ==0)
lh14g13 9:22b119eef1de 172 {
lh14g13 9:22b119eef1de 173
lh14g13 9:22b119eef1de 174 w-= change;
lh14g13 9:22b119eef1de 175 }
lh14g13 9:22b119eef1de 176 return;
lh14g13 9:22b119eef1de 177 }
lh14g13 9:22b119eef1de 178
lh14g13 8:4df2a47ab4ee 179 //----------------------------------------------------------------------------------------------------------------------------
lh14g13 8:4df2a47ab4ee 180 //------------------------------------------------Cornering Control-----------------------------------------------------------
lh14g13 8:4df2a47ab4ee 181 //----------------------------------------------------------------------------------------------------------------------------
lh14g13 2:cc8ddc587af7 182
lh14g13 10:f4fc8ccde4ad 183
lh14g13 10:f4fc8ccde4ad 184
lh14g13 13:c43f157a6bac 185 // tThis is a function which works off of the duty cycle. NO SENSOR REQUIREMENT
lh14g13 13:c43f157a6bac 186 // CLEAN THIS UP AND UPDATE FOR SENSORS VERSION WITH THIS CODE
lh14g13 14:bc77edc4adb0 187 // this function works off the actual value rather than the change in angle. therefore need to have a variabe which stores where the value is.
lh14g13 11:4a6f97cc1f1e 188 void dutyCycleCorner( float speed, float angle)
lh14g13 10:f4fc8ccde4ad 189 {
lh14g13 11:4a6f97cc1f1e 190 bool leftOrRight;
lh14g13 11:4a6f97cc1f1e 191 if(theta<0)
lh14g13 11:4a6f97cc1f1e 192 {
lh14g13 14:bc77edc4adb0 193 leftOrRight=true;
lh14g13 14:bc77edc4adb0 194 theta=theta*-1;
lh14g13 11:4a6f97cc1f1e 195
lh14g13 11:4a6f97cc1f1e 196 }
lh14g13 14:bc77edc4adb0 197
lh14g13 13:c43f157a6bac 198 float deltaW = ((0.2f*tan((angle/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed;
lh14g13 11:4a6f97cc1f1e 199
lh14g13 14:bc77edc4adb0 200 //TFC_SetMotorPWM(w2,w1);
lh14g13 14:bc77edc4adb0 201 if(leftOrRight){
lh14g13 14:bc77edc4adb0 202 TFC_SetMotorPWM(speed+ deltaW,speed- deltaW);
lh14g13 11:4a6f97cc1f1e 203 }
lh14g13 11:4a6f97cc1f1e 204 else{
lh14g13 14:bc77edc4adb0 205 TFC_SetMotorPWM(speed- deltaW,speed+ deltaW);
lh14g13 14:bc77edc4adb0 206 }
lh14g13 10:f4fc8ccde4ad 207
lh14g13 10:f4fc8ccde4ad 208
lh14g13 10:f4fc8ccde4ad 209
lh14g13 10:f4fc8ccde4ad 210 return;
lh14g13 10:f4fc8ccde4ad 211 }
lh14g13 10:f4fc8ccde4ad 212
lh14g13 2:cc8ddc587af7 213
lh14g13 14:bc77edc4adb0 214 void sensorCorner(float &w1,float &w2,float deltaTheta,int maxspeed)
lh14g13 14:bc77edc4adb0 215 { // when cornering left the left motor slows down more than the right hand side
lh14g13 2:cc8ddc587af7 216 // may just replace with ACC and DECC
lh14g13 14:bc77edc4adb0 217
lh14g13 14:bc77edc4adb0 218 bool leftOrRight;
lh14g13 11:4a6f97cc1f1e 219
lh14g13 14:bc77edc4adb0 220 if(theta<0)
lh14g13 14:bc77edc4adb0 221 {
lh14g13 14:bc77edc4adb0 222 leftOrRight=true;
lh14g13 14:bc77edc4adb0 223 theta=theta*-1;
lh14g13 14:bc77edc4adb0 224
lh14g13 14:bc77edc4adb0 225 }
lh14g13 14:bc77edc4adb0 226
lh14g13 14:bc77edc4adb0 227 float deltaW = ((0.2f*tan((angle/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed;
lh14g13 3:5b5d5af46804 228
lh14g13 14:bc77edc4adb0 229 &w1= maxspeed+ deltaW;
lh14g13 14:bc77edc4adb0 230 &w2= maxspeed -deltaW;
lh14g13 2:cc8ddc587af7 231
lh14g13 2:cc8ddc587af7 232
lh14g13 14:bc77edc4adb0 233 return;
lh14g13 14:bc77edc4adb0 234 }
lh14g13 2:cc8ddc587af7 235
lh14g13 7:9aaa4f73bb32 236
lh14g13 7:9aaa4f73bb32 237
lh14g13 5:c50e40797114 238
lh14g13 7:9aaa4f73bb32 239 float centerWheels(float min, float max)
lh14g13 7:9aaa4f73bb32 240 {
lh14g13 7:9aaa4f73bb32 241 float center= (min +max)/2;
lh14g13 7:9aaa4f73bb32 242
lh14g13 7:9aaa4f73bb32 243 return center;
lh14g13 7:9aaa4f73bb32 244 }
lh14g13 7:9aaa4f73bb32 245
lh14g13 8:4df2a47ab4ee 246 void steering(float center, float theta, int maxspeed,float & w1, float & w2)
lh14g13 8:4df2a47ab4ee 247 { // this function is for simply calculating the motor speeds. this reduces the amount of calculations needed
lh14g13 8:4df2a47ab4ee 248 //and can be triggered when the car steers.
lh14g13 8:4df2a47ab4ee 249
lh14g13 8:4df2a47ab4ee 250
lh14g13 7:9aaa4f73bb32 251 float deltaTheta = center- theta;
lh14g13 9:22b119eef1de 252
lh14g13 9:22b119eef1de 253 // need to convert to degrees/radians. that can do the above as well.
lh14g13 7:9aaa4f73bb32 254 if(deltaTheta <0)
lh14g13 7:9aaa4f73bb32 255 {
lh14g13 7:9aaa4f73bb32 256 // going left?
lh14g13 8:4df2a47ab4ee 257 corner(w1,w2,deltaTheta,maxspeed);
lh14g13 5:c50e40797114 258 }
lh14g13 2:cc8ddc587af7 259
lh14g13 11:4a6f97cc1f1e 260 else{
lh14g13 7:9aaa4f73bb32 261 //going right?
lh14g13 11:4a6f97cc1f1e 262 corner(w2,w1,deltaTheta,maxspeed);
lh14g13 7:9aaa4f73bb32 263
lh14g13 7:9aaa4f73bb32 264 }
lh14g13 2:cc8ddc587af7 265
lh14g13 7:9aaa4f73bb32 266 return;
lh14g13 14:bc77edc4adb0 267 }
lh14g13 14:bc77edc4adb0 268
lh14g13 14:bc77edc4adb0 269 void setDutyCycle(int w, int targetW)
lh14g13 14:bc77edc4adb0 270 {
lh14g13 14:bc77edc4adb0 271
lh14g13 14:bc77edc4adb0 272
lh14g13 14:bc77edc4adb0 273
lh14g13 14:bc77edc4adb0 274
lh14g13 14:bc77edc4adb0 275 }
lh14g13 14:bc77edc4adb0 276
lh14g13 14:bc77edc4adb0 277
lh14g13 14:bc77edc4adb0 278
lh14g13 14:bc77edc4adb0 279