GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@8:4df2a47ab4ee, 2016-11-07 (annotated)

Committer:
lh14g13
Date:
Mon Nov 07 16:12:48 2016 +0000
Branch:
testing
Revision:
8:4df2a47ab4ee
Parent:
7:9aaa4f73bb32
Child:
9:22b119eef1de
made sure the angular speeds are referenced

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 8:4df2a47ab4ee 18
lh14g13 8:4df2a47ab4ee 19 void corner(float &w1,float &w2,float deltaTheta,int maxspeed)
lh14g13 0:b0476dcfa14c 20
lh14g13 7:9aaa4f73bb32 21 void steering(float center, float theta, int maxspeed);
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 7:9aaa4f73bb32 31
lh14g13 7:9aaa4f73bb32 32 //need a function for calcu;lating the angle
lh14g13 7:9aaa4f73bb32 33 //need a function for converting w1 to delta. or do i?
lh14g13 0:b0476dcfa14c 34
lh14g13 0:b0476dcfa14c 35
lh14g13 0:b0476dcfa14c 36
lh14g13 7:9aaa4f73bb32 37 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 38 {
lh14g13 0:b0476dcfa14c 39
lh14g13 7:9aaa4f73bb32 40
lh14g13 7:9aaa4f73bb32 41 TFC_SetMotorPWM(0.4,0.7);
lh14g13 0:b0476dcfa14c 42
lh14g13 0:b0476dcfa14c 43 while(1)
lh14g13 0:b0476dcfa14c 44 {
lh14g13 0:b0476dcfa14c 45 if(TFC_ReadPushButton(0)>0)
lh14g13 0:b0476dcfa14c 46 {
lh14g13 0:b0476dcfa14c 47 TFC_SetMotorPWM(0.0,0.0);
lh14g13 7:9aaa4f73bb32 48
lh14g13 0:b0476dcfa14c 49 DefaultMode();
lh14g13 0:b0476dcfa14c 50
lh14g13 0:b0476dcfa14c 51 }
lh14g13 0:b0476dcfa14c 52 }
lh14g13 0:b0476dcfa14c 53 return;
lh14g13 0:b0476dcfa14c 54 }
lh14g13 0:b0476dcfa14c 55
lh14g13 0:b0476dcfa14c 56 void DefaultMode()
lh14g13 0:b0476dcfa14c 57 {
lh14g13 7:9aaa4f73bb32 58 TFC_Init();
lh14g13 0:b0476dcfa14c 59 while(1)
lh14g13 0:b0476dcfa14c 60 {
lh14g13 7:9aaa4f73bb32 61 TFC_HBRIDGE_ENABLE;
lh14g13 0:b0476dcfa14c 62 if(TFC_ReadPushButton(1)>0)
lh14g13 0:b0476dcfa14c 63 {
lh14g13 7:9aaa4f73bb32 64 runMotor();
lh14g13 0:b0476dcfa14c 65 }
lh14g13 0:b0476dcfa14c 66
lh14g13 0:b0476dcfa14c 67 else if(TFC_ReadPushButton(0)>0)
lh14g13 0:b0476dcfa14c 68 {
lh14g13 0:b0476dcfa14c 69 //this will be a debug mode
lh14g13 0:b0476dcfa14c 70 }
lh14g13 0:b0476dcfa14c 71
lh14g13 0:b0476dcfa14c 72
lh14g13 0:b0476dcfa14c 73 }
lh14g13 7:9aaa4f73bb32 74 TFC_HBRIDGE_DISABLE;
lh14g13 0:b0476dcfa14c 75 return;
lh14g13 0:b0476dcfa14c 76 }
lh14g13 0:b0476dcfa14c 77
lh14g13 0:b0476dcfa14c 78
lh14g13 0:b0476dcfa14c 79 void Acc(float& motorA, float& motorB)// set up so as to control both motors during acc. Potential use during corners
lh14g13 0:b0476dcfa14c 80 {
lh14g13 0:b0476dcfa14c 81 motorA = motorA + 0.1;
lh14g13 0:b0476dcfa14c 82 motorB = motorB + 0.1;
lh14g13 0:b0476dcfa14c 83 TFC_SetMotorPWM(motorA,motorB);
lh14g13 0:b0476dcfa14c 84
lh14g13 0:b0476dcfa14c 85
lh14g13 0:b0476dcfa14c 86 return ;
lh14g13 0:b0476dcfa14c 87
lh14g13 0:b0476dcfa14c 88 }
lh14g13 0:b0476dcfa14c 89
lh14g13 0:b0476dcfa14c 90 void Decc(float &motorA, float &motorB)
lh14g13 0:b0476dcfa14c 91 {
lh14g13 0:b0476dcfa14c 92 // 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 93
lh14g13 0:b0476dcfa14c 94 motorA = motorA - 0.1;
lh14g13 0:b0476dcfa14c 95 motorB = motorB - 0.1;
lh14g13 0:b0476dcfa14c 96
lh14g13 0:b0476dcfa14c 97 TFC_SetMotorPWM(motorA,motorB);
lh14g13 0:b0476dcfa14c 98
lh14g13 0:b0476dcfa14c 99 return ;
lh14g13 0:b0476dcfa14c 100
lh14g13 0:b0476dcfa14c 101 }
lh14g13 0:b0476dcfa14c 102
lh14g13 0:b0476dcfa14c 103
lh14g13 0:b0476dcfa14c 104 void StartLine()
lh14g13 0:b0476dcfa14c 105 {
lh14g13 0:b0476dcfa14c 106 TFC_HBRIDGE_ENABLE;
lh14g13 0:b0476dcfa14c 107 float a=0;
lh14g13 0:b0476dcfa14c 108 float b=0;
lh14g13 0:b0476dcfa14c 109
lh14g13 0:b0476dcfa14c 110 int x=0 ;
lh14g13 0:b0476dcfa14c 111 while(x<5)
lh14g13 0:b0476dcfa14c 112 {
lh14g13 0:b0476dcfa14c 113 Acc(a,b);
lh14g13 0:b0476dcfa14c 114 wait(0.5);
lh14g13 0:b0476dcfa14c 115 x++ ;
lh14g13 0:b0476dcfa14c 116 }
lh14g13 0:b0476dcfa14c 117
lh14g13 0:b0476dcfa14c 118 return ;
lh14g13 0:b0476dcfa14c 119 }
lh14g13 0:b0476dcfa14c 120
lh14g13 0:b0476dcfa14c 121 void finishLine(float pwmA)
lh14g13 0:b0476dcfa14c 122 {
lh14g13 0:b0476dcfa14c 123 float pwmB= pwmA;
lh14g13 0:b0476dcfa14c 124 while(pwmA>0)
lh14g13 0:b0476dcfa14c 125 {
lh14g13 0:b0476dcfa14c 126
lh14g13 0:b0476dcfa14c 127
lh14g13 0:b0476dcfa14c 128 Decc(pwmA,pwmB);
lh14g13 0:b0476dcfa14c 129
lh14g13 0:b0476dcfa14c 130 }
lh14g13 0:b0476dcfa14c 131 return;
lh14g13 0:b0476dcfa14c 132 }
lh14g13 0:b0476dcfa14c 133
lh14g13 0:b0476dcfa14c 134
lh14g13 8:4df2a47ab4ee 135 //----------------------------------------------------------------------------------------------------------------------------
lh14g13 8:4df2a47ab4ee 136 //------------------------------------------------Cornering Control-----------------------------------------------------------
lh14g13 8:4df2a47ab4ee 137 //----------------------------------------------------------------------------------------------------------------------------
lh14g13 2:cc8ddc587af7 138
lh14g13 2:cc8ddc587af7 139
lh14g13 8:4df2a47ab4ee 140 void corner(float &w1,float &w2,float deltaTheta,int maxspeed)
lh14g13 2:cc8ddc587af7 141 {// when cornering left the left motor slows down more than the right hand side
lh14g13 2:cc8ddc587af7 142 // may just replace with ACC and DECC
lh14g13 3:5b5d5af46804 143 float r;
lh14g13 3:5b5d5af46804 144 float d;
lh14g13 3:5b5d5af46804 145 float l;
lh14g13 2:cc8ddc587af7 146 // it may be worth doing this off the change in theta. so that it is insesnsitive to the calibration.
lh14g13 4:e15ec9052a78 147 float diff= ((d*tan(deltaTheta)/(2*l)));
lh14g13 3:5b5d5af46804 148
lh14g13 8:4df2a47ab4ee 149 float w1 = (maxspeed/r)*(1+diff);
lh14g13 8:4df2a47ab4ee 150 float w2 = (maxspeed/r)*(1-diff);
lh14g13 3:5b5d5af46804 151
lh14g13 8:4df2a47ab4ee 152
lh14g13 2:cc8ddc587af7 153
lh14g13 2:cc8ddc587af7 154
lh14g13 2:cc8ddc587af7 155 return;
lh14g13 2:cc8ddc587af7 156 }
lh14g13 2:cc8ddc587af7 157
lh14g13 7:9aaa4f73bb32 158
lh14g13 7:9aaa4f73bb32 159
lh14g13 5:c50e40797114 160
lh14g13 7:9aaa4f73bb32 161 float centerWheels(float min, float max)
lh14g13 7:9aaa4f73bb32 162 {
lh14g13 7:9aaa4f73bb32 163 float center= (min +max)/2;
lh14g13 7:9aaa4f73bb32 164
lh14g13 7:9aaa4f73bb32 165 return center;
lh14g13 7:9aaa4f73bb32 166 }
lh14g13 7:9aaa4f73bb32 167
lh14g13 8:4df2a47ab4ee 168 void steering(float center, float theta, int maxspeed,float & w1, float & w2)
lh14g13 8:4df2a47ab4ee 169 { // this function is for simply calculating the motor speeds. this reduces the amount of calculations needed
lh14g13 8:4df2a47ab4ee 170 //and can be triggered when the car steers.
lh14g13 8:4df2a47ab4ee 171
lh14g13 8:4df2a47ab4ee 172
lh14g13 7:9aaa4f73bb32 173 float deltaTheta = center- theta;
lh14g13 7:9aaa4f73bb32 174 if(deltaTheta <0)
lh14g13 7:9aaa4f73bb32 175 {
lh14g13 7:9aaa4f73bb32 176 // going left?
lh14g13 8:4df2a47ab4ee 177 corner(w1,w2,deltaTheta,maxspeed);
lh14g13 5:c50e40797114 178 }
lh14g13 2:cc8ddc587af7 179
lh14g13 7:9aaa4f73bb32 180 else()
lh14g13 7:9aaa4f73bb32 181 {
lh14g13 7:9aaa4f73bb32 182 //going right?
lh14g13 7:9aaa4f73bb32 183 corner(w2,w1,deltaTheta)
lh14g13 7:9aaa4f73bb32 184
lh14g13 7:9aaa4f73bb32 185 }
lh14g13 2:cc8ddc587af7 186
lh14g13 7:9aaa4f73bb32 187 return;
lh14g13 7:9aaa4f73bb32 188 }