GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@45:fec835dfd43a, 2017-04-27 (annotated)

Committer:
lh14g13
Date:
Thu Apr 27 15:50:28 2017 +0000
Branch:
motorupdate
Revision:
45:fec835dfd43a
Parent:
44:b46efc310b83
Child:
47:bafec50148b6
added ability to shift to torque; ;

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 15:f40e834d063b 111 void dutyCycleCorner( float speed, float theta)
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 29:d3efef939c18 121 //calculates the difference in
lh14g13 18:e3fd26490f58 122 float deltaW = ((0.1f*tan((theta/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed;
lh14g13 11:4a6f97cc1f1e 123
lh14g13 14:bc77edc4adb0 124 //TFC_SetMotorPWM(w2,w1);
lh14g13 28:9d4042b05640 125 // this sets the speeds of each motor (outer wheel goes faster than inner wheel)
lh14g13 14:bc77edc4adb0 126 if(leftOrRight){
lh14g13 14:bc77edc4adb0 127 TFC_SetMotorPWM(speed+ deltaW,speed- deltaW);
lh14g13 11:4a6f97cc1f1e 128 }
lh14g13 11:4a6f97cc1f1e 129 else{
lh14g13 14:bc77edc4adb0 130 TFC_SetMotorPWM(speed- deltaW,speed+ deltaW);
lh14g13 14:bc77edc4adb0 131 }
lh14g13 10:f4fc8ccde4ad 132
lh14g13 10:f4fc8ccde4ad 133
lh14g13 10:f4fc8ccde4ad 134
lh14g13 10:f4fc8ccde4ad 135 return;
lh14g13 10:f4fc8ccde4ad 136 }
lh14g13 10:f4fc8ccde4ad 137
lh14g13 2:cc8ddc587af7 138
lh14g13 33:734ce4c36c3f 139 void sensorCorner(float &w1,float &w2,float theta,float speed, float tune)
lh14g13 14:bc77edc4adb0 140 { // when cornering left the left motor slows down more than the right hand side
lh14g13 27:98aecf1889ed 141 // this is the ED for when the car is running off of the sensors rather than a set PWM
lh14g13 14:bc77edc4adb0 142
lh14g13 24:15c6bbdbb0e4 143 bool leftOrRight = false;
lh14g13 36:8b6c896e1ab5 144
lh14g13 28:9d4042b05640 145 // makes theta positive and sets if the car is turning left or right
lh14g13 14:bc77edc4adb0 146 if(theta<0)
lh14g13 14:bc77edc4adb0 147 {
lh14g13 14:bc77edc4adb0 148 leftOrRight=true;
lh14g13 45:fec835dfd43a 149 theta = theta*-1;
lh14g13 14:bc77edc4adb0 150
lh14g13 14:bc77edc4adb0 151 }
lh14g13 24:15c6bbdbb0e4 152
lh14g13 29:d3efef939c18 153 //this limits the ED to stop it spinning out at high speeds.
lh14g13 36:8b6c896e1ab5 154
lh14g13 44:b46efc310b83 155
lh14g13 45:fec835dfd43a 156 if(theta>0.35)
lh14g13 24:15c6bbdbb0e4 157 {
lh14g13 36:8b6c896e1ab5 158
lh14g13 45:fec835dfd43a 159 theta = 0.35;
lh14g13 45:fec835dfd43a 160 //speed = speed- cornerlimiter;
lh14g13 24:15c6bbdbb0e4 161
lh14g13 24:15c6bbdbb0e4 162 }
lh14g13 27:98aecf1889ed 163
lh14g13 27:98aecf1889ed 164
lh14g13 27:98aecf1889ed 165
lh14g13 3:5b5d5af46804 166
lh14g13 27:98aecf1889ed 167 //These equations set the angular speeds of the motors
lh14g13 27:98aecf1889ed 168 //there are two equations for testing purposes
lh14g13 26:c74e70a745ec 169 if(true){
lh14g13 28:9d4042b05640 170 // calculates the difference in the speed
lh14g13 38:4bb39bb1e685 171 float angle= tan((theta/0.02222f)* (3.14f / 180.0f));
lh14g13 38:4bb39bb1e685 172 float deltaW = (0.1f/(2*0.2f*angle))*speed;
lh14g13 28:9d4042b05640 173 //
lh14g13 20:ca87a36c7688 174 if(leftOrRight){
lh14g13 45:fec835dfd43a 175 w1= speed + (deltaW * tune);
lh14g13 41:827ff8733729 176 w2= speed -(deltaW * tune);
lh14g13 27:98aecf1889ed 177
lh14g13 25:255d169a45e1 178
lh14g13 21:aaa482dad274 179 }
lh14g13 21:aaa482dad274 180 else{
lh14g13 43:9384715df1b8 181 w1= speed- (deltaW*tune);
lh14g13 43:9384715df1b8 182 w2= speed+ (deltaW*tune);
lh14g13 27:98aecf1889ed 183
lh14g13 20:ca87a36c7688 184 }
lh14g13 26:c74e70a745ec 185 }
lh14g13 27:98aecf1889ed 186
lh14g13 40:29373275b6cb 187 /* else{
lh14g13 33:734ce4c36c3f 188 //calulates the speed in eack wheel
lh14g13 33:734ce4c36c3f 189 //speed of inner wheel
lh14g13 38:4bb39bb1e685 190 float vin = speed*(1 - 0.1/tan((theta/0.022222)*(3.14f / 180.0f)));
lh14g13 33:734ce4c36c3f 191 //speed of outer wheel.
lh14g13 38:4bb39bb1e685 192 float vout = speed*(1 + 0.1/tan((theta/0.022222)*(3.14f / 180.0f)));
lh14g13 39:772ff68123cc 193
lh14g13 39:772ff68123cc 194 // this sets the speeds of the inner and outer wheels
lh14g13 27:98aecf1889ed 195 if(leftOrRight){
lh14g13 26:c74e70a745ec 196
lh14g13 26:c74e70a745ec 197 w1=vout;
lh14g13 33:734ce4c36c3f 198
lh14g13 26:c74e70a745ec 199 w2=vin;
lh14g13 26:c74e70a745ec 200
lh14g13 26:c74e70a745ec 201 }
lh14g13 26:c74e70a745ec 202 else{
lh14g13 26:c74e70a745ec 203
lh14g13 26:c74e70a745ec 204 w1=vin;
lh14g13 26:c74e70a745ec 205 w2=vout;
lh14g13 26:c74e70a745ec 206 }
lh14g13 2:cc8ddc587af7 207
lh14g13 40:29373275b6cb 208 }*/
lh14g13 14:bc77edc4adb0 209 return;
lh14g13 14:bc77edc4adb0 210 }
lh14g13 2:cc8ddc587af7 211
lh14g13 7:9aaa4f73bb32 212
lh14g13 7:9aaa4f73bb32 213
lh14g13 5:c50e40797114 214
lh14g13 14:bc77edc4adb0 215
lh14g13 14:bc77edc4adb0 216
lh14g13 14:bc77edc4adb0 217
lh14g13 14:bc77edc4adb0 218
lh14g13 15:f40e834d063b 219
lh14g13 15:f40e834d063b 220
lh14g13 15:f40e834d063b 221
lh14g13 15:f40e834d063b 222