GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@48:2e39f9fca850, 2017-04-28 (annotated)

Committer:
lh14g13
Date:
Fri Apr 28 07:54:55 2017 +0000
Branch:
motorupdate
Revision:
48:2e39f9fca850
Parent:
47:bafec50148b6
Child:
49:98fb0d816007
fixed corner limiter error

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>
FatCookies 22:85cf7b6b0422 4 #include "motor.h"
lh14g13 0:b0476dcfa14c 5 DigitalOut myled(LED1);
lh14g13 0:b0476dcfa14c 6
lh14g13 27:98aecf1889ed 7
lh14g13 0:b0476dcfa14c 8
lh14g13 0:b0476dcfa14c 9
lh14g13 15:f40e834d063b 10
lh14g13 16:9c5e9306ae10 11 //Variables
lh14g13 16:9c5e9306ae10 12 int cornerPwmControl;
lh14g13 0:b0476dcfa14c 13
lh14g13 0:b0476dcfa14c 14
lh14g13 0:b0476dcfa14c 15
lh14g13 0:b0476dcfa14c 16 // uncomment for testing motor functions.
lh14g13 0:b0476dcfa14c 17
lh14g13 0:b0476dcfa14c 18
lh14g13 7:9aaa4f73bb32 19
lh14g13 7:9aaa4f73bb32 20 //need a function for calcu;lating the angle
lh14g13 7:9aaa4f73bb32 21 //need a function for converting w1 to delta. or do i?
lh14g13 0:b0476dcfa14c 22
lh14g13 10:f4fc8ccde4ad 23 //----------------------------------------------------------------------------------------------------------------------------------
lh14g13 10:f4fc8ccde4ad 24 //----------------------------------------------This is for Motor Set up------------------------------------------------------------
lh14g13 10:f4fc8ccde4ad 25 //----------------------------------------------------------------------------------------------------------------------------------
lh14g13 0:b0476dcfa14c 26
lh14g13 45:fec835dfd43a 27 void runMotor() // This simply sets the PWM and turns the motors off when the button is pushed
lh14g13 0:b0476dcfa14c 28 {
lh14g13 0:b0476dcfa14c 29
lh14g13 7:9aaa4f73bb32 30
lh14g13 27:98aecf1889ed 31 TFC_SetMotorPWM(0.4,0.4);
lh14g13 0:b0476dcfa14c 32
lh14g13 0:b0476dcfa14c 33 while(1)
lh14g13 0:b0476dcfa14c 34 {
lh14g13 0:b0476dcfa14c 35 if(TFC_ReadPushButton(0)>0)
lh14g13 0:b0476dcfa14c 36 {
lh14g13 0:b0476dcfa14c 37 TFC_SetMotorPWM(0.0,0.0);
lh14g13 7:9aaa4f73bb32 38
lh14g13 0:b0476dcfa14c 39 DefaultMode();
lh14g13 0:b0476dcfa14c 40
lh14g13 0:b0476dcfa14c 41 }
lh14g13 0:b0476dcfa14c 42 }
lh14g13 0:b0476dcfa14c 43 return;
lh14g13 0:b0476dcfa14c 44 }
lh14g13 27:98aecf1889ed 45 // This works for starting the car using the buttons on the bridge
lh14g13 0:b0476dcfa14c 46 void DefaultMode()
lh14g13 0:b0476dcfa14c 47 {
lh14g13 7:9aaa4f73bb32 48 TFC_Init();
lh14g13 0:b0476dcfa14c 49 while(1)
lh14g13 0:b0476dcfa14c 50 {
lh14g13 7:9aaa4f73bb32 51 TFC_HBRIDGE_ENABLE;
lh14g13 0:b0476dcfa14c 52 if(TFC_ReadPushButton(1)>0)
lh14g13 0:b0476dcfa14c 53 {
lh14g13 7:9aaa4f73bb32 54 runMotor();
lh14g13 0:b0476dcfa14c 55 }
lh14g13 0:b0476dcfa14c 56
lh14g13 0:b0476dcfa14c 57 else if(TFC_ReadPushButton(0)>0)
lh14g13 0:b0476dcfa14c 58 {
lh14g13 0:b0476dcfa14c 59 //this will be a debug mode
lh14g13 0:b0476dcfa14c 60 }
lh14g13 0:b0476dcfa14c 61
lh14g13 0:b0476dcfa14c 62
lh14g13 0:b0476dcfa14c 63 }
lh14g13 7:9aaa4f73bb32 64 TFC_HBRIDGE_DISABLE;
lh14g13 0:b0476dcfa14c 65 return;
lh14g13 0:b0476dcfa14c 66 }
lh14g13 10:f4fc8ccde4ad 67 //-----------------------------------------------------------------------------------------------------
lh14g13 10:f4fc8ccde4ad 68 //------------------------ this is for speed control---------------------------------------------------
lh14g13 10:f4fc8ccde4ad 69 //-----------------------------------------------------------------------------------------------------
lh14g13 27:98aecf1889ed 70 // This simply sets the duty cycle.
lh14g13 27:98aecf1889ed 71 // its a rudementary motor control.
lh14g13 15:f40e834d063b 72 float setDutyCycle(float dutyC, int w,int targetW)
lh14g13 15:f40e834d063b 73 {
lh14g13 0:b0476dcfa14c 74
lh14g13 0:b0476dcfa14c 75
lh14g13 15:f40e834d063b 76 if(true)
lh14g13 0:b0476dcfa14c 77 {
lh14g13 15:f40e834d063b 78 if(w<targetW)
lh14g13 15:f40e834d063b 79 {
lh14g13 15:f40e834d063b 80 dutyC+=0.1;
lh14g13 0:b0476dcfa14c 81
lh14g13 15:f40e834d063b 82 }
lh14g13 0:b0476dcfa14c 83
lh14g13 15:f40e834d063b 84 else
lh14g13 15:f40e834d063b 85 {
lh14g13 15:f40e834d063b 86 dutyC-=0.1;
lh14g13 15:f40e834d063b 87 }
lh14g13 0:b0476dcfa14c 88
lh14g13 0:b0476dcfa14c 89 }
lh14g13 0:b0476dcfa14c 90
lh14g13 0:b0476dcfa14c 91
lh14g13 15:f40e834d063b 92 else if(false)
lh14g13 0:b0476dcfa14c 93 {
lh14g13 0:b0476dcfa14c 94
lh14g13 0:b0476dcfa14c 95 }
lh14g13 0:b0476dcfa14c 96
lh14g13 15:f40e834d063b 97
lh14g13 15:f40e834d063b 98 return dutyC;
lh14g13 9:22b119eef1de 99 }
lh14g13 9:22b119eef1de 100
lh14g13 15:f40e834d063b 101
lh14g13 8:4df2a47ab4ee 102 //----------------------------------------------------------------------------------------------------------------------------
lh14g13 8:4df2a47ab4ee 103 //------------------------------------------------Cornering Control-----------------------------------------------------------
lh14g13 8:4df2a47ab4ee 104 //----------------------------------------------------------------------------------------------------------------------------
lh14g13 2:cc8ddc587af7 105
lh14g13 10:f4fc8ccde4ad 106
lh14g13 10:f4fc8ccde4ad 107
lh14g13 27:98aecf1889ed 108 // This is a function which works off of the duty cycle. NO SENSOR REQUIREMENT
lh14g13 27:98aecf1889ed 109
lh14g13 14:bc77edc4adb0 110 // 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 47:bafec50148b6 111 void dutyCycleCorner( float speed, float theta,float tuner)
lh14g13 10:f4fc8ccde4ad 112 {
lh14g13 11:4a6f97cc1f1e 113 bool leftOrRight;
lh14g13 28:9d4042b05640 114 // sets if the car is going left or right
lh14g13 11:4a6f97cc1f1e 115 if(theta<0)
lh14g13 11:4a6f97cc1f1e 116 {
lh14g13 14:bc77edc4adb0 117 leftOrRight=true;
lh14g13 14:bc77edc4adb0 118 theta=theta*-1;
lh14g13 11:4a6f97cc1f1e 119
lh14g13 11:4a6f97cc1f1e 120 }
lh14g13 48:2e39f9fca850 121 if(theta>0.3)
lh14g13 48:2e39f9fca850 122 {
lh14g13 48:2e39f9fca850 123 theta =0.3;
lh14g13 48:2e39f9fca850 124
lh14g13 48:2e39f9fca850 125 }
lh14g13 29:d3efef939c18 126 //calculates the difference in
lh14g13 47:bafec50148b6 127 float deltaW = (((0.1f*tan((theta/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed)*tuner;
lh14g13 11:4a6f97cc1f1e 128
lh14g13 14:bc77edc4adb0 129 //TFC_SetMotorPWM(w2,w1);
lh14g13 28:9d4042b05640 130 // this sets the speeds of each motor (outer wheel goes faster than inner wheel)
lh14g13 14:bc77edc4adb0 131 if(leftOrRight){
lh14g13 14:bc77edc4adb0 132 TFC_SetMotorPWM(speed+ deltaW,speed- deltaW);
lh14g13 11:4a6f97cc1f1e 133 }
lh14g13 11:4a6f97cc1f1e 134 else{
lh14g13 14:bc77edc4adb0 135 TFC_SetMotorPWM(speed- deltaW,speed+ deltaW);
lh14g13 14:bc77edc4adb0 136 }
lh14g13 10:f4fc8ccde4ad 137
lh14g13 10:f4fc8ccde4ad 138
lh14g13 10:f4fc8ccde4ad 139
lh14g13 10:f4fc8ccde4ad 140 return;
lh14g13 10:f4fc8ccde4ad 141 }
lh14g13 10:f4fc8ccde4ad 142
lh14g13 2:cc8ddc587af7 143
lh14g13 33:734ce4c36c3f 144 void sensorCorner(float &w1,float &w2,float theta,float speed, float tune)
lh14g13 14:bc77edc4adb0 145 { // when cornering left the left motor slows down more than the right hand side
lh14g13 27:98aecf1889ed 146 // this is the ED for when the car is running off of the sensors rather than a set PWM
lh14g13 14:bc77edc4adb0 147
lh14g13 24:15c6bbdbb0e4 148 bool leftOrRight = false;
lh14g13 36:8b6c896e1ab5 149
lh14g13 28:9d4042b05640 150 // makes theta positive and sets if the car is turning left or right
lh14g13 14:bc77edc4adb0 151 if(theta<0)
lh14g13 14:bc77edc4adb0 152 {
lh14g13 14:bc77edc4adb0 153 leftOrRight=true;
lh14g13 45:fec835dfd43a 154 theta = theta*-1;
lh14g13 14:bc77edc4adb0 155
lh14g13 14:bc77edc4adb0 156 }
lh14g13 24:15c6bbdbb0e4 157
lh14g13 29:d3efef939c18 158 //this limits the ED to stop it spinning out at high speeds.
lh14g13 36:8b6c896e1ab5 159
lh14g13 44:b46efc310b83 160
lh14g13 45:fec835dfd43a 161 if(theta>0.35)
lh14g13 24:15c6bbdbb0e4 162 {
lh14g13 36:8b6c896e1ab5 163
lh14g13 45:fec835dfd43a 164 theta = 0.35;
lh14g13 45:fec835dfd43a 165 //speed = speed- cornerlimiter;
lh14g13 24:15c6bbdbb0e4 166
lh14g13 24:15c6bbdbb0e4 167 }
lh14g13 27:98aecf1889ed 168
lh14g13 27:98aecf1889ed 169
lh14g13 27:98aecf1889ed 170
lh14g13 3:5b5d5af46804 171
lh14g13 27:98aecf1889ed 172 //These equations set the angular speeds of the motors
lh14g13 27:98aecf1889ed 173 //there are two equations for testing purposes
lh14g13 26:c74e70a745ec 174 if(true){
lh14g13 28:9d4042b05640 175 // calculates the difference in the speed
lh14g13 38:4bb39bb1e685 176 float angle= tan((theta/0.02222f)* (3.14f / 180.0f));
lh14g13 38:4bb39bb1e685 177 float deltaW = (0.1f/(2*0.2f*angle))*speed;
lh14g13 28:9d4042b05640 178 //
lh14g13 20:ca87a36c7688 179 if(leftOrRight){
lh14g13 45:fec835dfd43a 180 w1= speed + (deltaW * tune);
lh14g13 41:827ff8733729 181 w2= speed -(deltaW * tune);
lh14g13 27:98aecf1889ed 182
lh14g13 25:255d169a45e1 183
lh14g13 21:aaa482dad274 184 }
lh14g13 21:aaa482dad274 185 else{
lh14g13 43:9384715df1b8 186 w1= speed- (deltaW*tune);
lh14g13 43:9384715df1b8 187 w2= speed+ (deltaW*tune);
lh14g13 27:98aecf1889ed 188
lh14g13 20:ca87a36c7688 189 }
lh14g13 26:c74e70a745ec 190 }
lh14g13 27:98aecf1889ed 191
lh14g13 40:29373275b6cb 192 /* else{
lh14g13 33:734ce4c36c3f 193 //calulates the speed in eack wheel
lh14g13 33:734ce4c36c3f 194 //speed of inner wheel
lh14g13 38:4bb39bb1e685 195 float vin = speed*(1 - 0.1/tan((theta/0.022222)*(3.14f / 180.0f)));
lh14g13 33:734ce4c36c3f 196 //speed of outer wheel.
lh14g13 38:4bb39bb1e685 197 float vout = speed*(1 + 0.1/tan((theta/0.022222)*(3.14f / 180.0f)));
lh14g13 39:772ff68123cc 198
lh14g13 39:772ff68123cc 199 // this sets the speeds of the inner and outer wheels
lh14g13 27:98aecf1889ed 200 if(leftOrRight){
lh14g13 26:c74e70a745ec 201
lh14g13 26:c74e70a745ec 202 w1=vout;
lh14g13 33:734ce4c36c3f 203
lh14g13 26:c74e70a745ec 204 w2=vin;
lh14g13 26:c74e70a745ec 205
lh14g13 26:c74e70a745ec 206 }
lh14g13 26:c74e70a745ec 207 else{
lh14g13 26:c74e70a745ec 208
lh14g13 26:c74e70a745ec 209 w1=vin;
lh14g13 26:c74e70a745ec 210 w2=vout;
lh14g13 26:c74e70a745ec 211 }
lh14g13 2:cc8ddc587af7 212
lh14g13 40:29373275b6cb 213 }*/
lh14g13 14:bc77edc4adb0 214 return;
lh14g13 14:bc77edc4adb0 215 }
lh14g13 2:cc8ddc587af7 216
lh14g13 7:9aaa4f73bb32 217
lh14g13 7:9aaa4f73bb32 218
lh14g13 5:c50e40797114 219
lh14g13 14:bc77edc4adb0 220
lh14g13 14:bc77edc4adb0 221
lh14g13 14:bc77edc4adb0 222
lh14g13 14:bc77edc4adb0 223
lh14g13 15:f40e834d063b 224
lh14g13 15:f40e834d063b 225
lh14g13 15:f40e834d063b 226
lh14g13 15:f40e834d063b 227