GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@42:80c4ce51cbe4, 2017-03-23 (annotated)

Committer:
lh14g13
Date:
Thu Mar 23 14:00:58 2017 +0000
Branch:
motorupdate
Revision:
42:80c4ce51cbe4
Parent:
41:827ff8733729
Child:
43:9384715df1b8
adjusted theta threshold

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 27:98aecf1889ed 27 void runMotor() // This simply sets the PWM and turns the motors off whent he 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 14:bc77edc4adb0 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 42:80c4ce51cbe4 155 if(theta>0.4)
lh14g13 24:15c6bbdbb0e4 156 {
lh14g13 36:8b6c896e1ab5 157
lh14g13 42:80c4ce51cbe4 158 theta = 0.4;
lh14g13 36:8b6c896e1ab5 159
lh14g13 24:15c6bbdbb0e4 160
lh14g13 24:15c6bbdbb0e4 161 }
lh14g13 27:98aecf1889ed 162
lh14g13 27:98aecf1889ed 163
lh14g13 27:98aecf1889ed 164
lh14g13 3:5b5d5af46804 165
lh14g13 27:98aecf1889ed 166 //These equations set the angular speeds of the motors
lh14g13 27:98aecf1889ed 167 //there are two equations for testing purposes
lh14g13 26:c74e70a745ec 168 if(true){
lh14g13 28:9d4042b05640 169 // calculates the difference in the speed
lh14g13 38:4bb39bb1e685 170 float angle= tan((theta/0.02222f)* (3.14f / 180.0f));
lh14g13 38:4bb39bb1e685 171 float deltaW = (0.1f/(2*0.2f*angle))*speed;
lh14g13 28:9d4042b05640 172 //
lh14g13 20:ca87a36c7688 173 if(leftOrRight){
lh14g13 41:827ff8733729 174 w1= speed+ (deltaW * tune);
lh14g13 41:827ff8733729 175 w2= speed -(deltaW * tune);
lh14g13 27:98aecf1889ed 176
lh14g13 25:255d169a45e1 177
lh14g13 21:aaa482dad274 178 }
lh14g13 21:aaa482dad274 179 else{
lh14g13 24:15c6bbdbb0e4 180 w1= speed- deltaW*tune;
lh14g13 24:15c6bbdbb0e4 181 w2= speed +deltaW*tune;
lh14g13 27:98aecf1889ed 182
lh14g13 20:ca87a36c7688 183 }
lh14g13 26:c74e70a745ec 184 }
lh14g13 27:98aecf1889ed 185
lh14g13 40:29373275b6cb 186 /* else{
lh14g13 33:734ce4c36c3f 187 //calulates the speed in eack wheel
lh14g13 33:734ce4c36c3f 188 //speed of inner wheel
lh14g13 38:4bb39bb1e685 189 float vin = speed*(1 - 0.1/tan((theta/0.022222)*(3.14f / 180.0f)));
lh14g13 33:734ce4c36c3f 190 //speed of outer wheel.
lh14g13 38:4bb39bb1e685 191 float vout = speed*(1 + 0.1/tan((theta/0.022222)*(3.14f / 180.0f)));
lh14g13 39:772ff68123cc 192
lh14g13 39:772ff68123cc 193 // this sets the speeds of the inner and outer wheels
lh14g13 27:98aecf1889ed 194 if(leftOrRight){
lh14g13 26:c74e70a745ec 195
lh14g13 26:c74e70a745ec 196 w1=vout;
lh14g13 33:734ce4c36c3f 197
lh14g13 26:c74e70a745ec 198 w2=vin;
lh14g13 26:c74e70a745ec 199
lh14g13 26:c74e70a745ec 200 }
lh14g13 26:c74e70a745ec 201 else{
lh14g13 26:c74e70a745ec 202
lh14g13 26:c74e70a745ec 203 w1=vin;
lh14g13 26:c74e70a745ec 204 w2=vout;
lh14g13 26:c74e70a745ec 205 }
lh14g13 2:cc8ddc587af7 206
lh14g13 40:29373275b6cb 207 }*/
lh14g13 14:bc77edc4adb0 208 return;
lh14g13 14:bc77edc4adb0 209 }
lh14g13 2:cc8ddc587af7 210
lh14g13 7:9aaa4f73bb32 211
lh14g13 7:9aaa4f73bb32 212
lh14g13 5:c50e40797114 213
lh14g13 14:bc77edc4adb0 214
lh14g13 14:bc77edc4adb0 215
lh14g13 14:bc77edc4adb0 216
lh14g13 14:bc77edc4adb0 217
lh14g13 15:f40e834d063b 218
lh14g13 15:f40e834d063b 219
lh14g13 15:f40e834d063b 220
lh14g13 15:f40e834d063b 221