GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@33:734ce4c36c3f, 2017-01-13 (annotated)

Committer:
lh14g13
Date:
Fri Jan 13 09:59:51 2017 +0000
Branch:
motorupdate
Revision:
33:734ce4c36c3f
Parent:
31:51783b84a859
Child:
35:f5933039b560
added the ability to tune the ED from outside the loop of the code

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 33:734ce4c36c3f 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 33:734ce4c36c3f 154 if(theta>0.4)
lh14g13 24:15c6bbdbb0e4 155 {
lh14g13 33:734ce4c36c3f 156 theta = 0.4;
lh14g13 24:15c6bbdbb0e4 157
lh14g13 24:15c6bbdbb0e4 158 }
lh14g13 27:98aecf1889ed 159
lh14g13 27:98aecf1889ed 160
lh14g13 27:98aecf1889ed 161
lh14g13 3:5b5d5af46804 162
lh14g13 27:98aecf1889ed 163 //These equations set the angular speeds of the motors
lh14g13 27:98aecf1889ed 164 //there are two equations for testing purposes
lh14g13 26:c74e70a745ec 165 if(true){
lh14g13 28:9d4042b05640 166 // calculates the difference in the speed
lh14g13 27:98aecf1889ed 167 float deltaW = ((0.1f*tan((theta/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed;
lh14g13 28:9d4042b05640 168 //
lh14g13 20:ca87a36c7688 169 if(leftOrRight){
lh14g13 24:15c6bbdbb0e4 170 w1= speed+ deltaW * tune;
lh14g13 24:15c6bbdbb0e4 171 w2= speed -deltaW * tune;
lh14g13 27:98aecf1889ed 172
lh14g13 25:255d169a45e1 173
lh14g13 21:aaa482dad274 174 }
lh14g13 21:aaa482dad274 175 else{
lh14g13 24:15c6bbdbb0e4 176 w1= speed- deltaW*tune;
lh14g13 24:15c6bbdbb0e4 177 w2= speed +deltaW*tune;
lh14g13 27:98aecf1889ed 178
lh14g13 20:ca87a36c7688 179 }
lh14g13 26:c74e70a745ec 180 }
lh14g13 27:98aecf1889ed 181
lh14g13 26:c74e70a745ec 182 else{
lh14g13 33:734ce4c36c3f 183 //calulates the speed in eack wheel
lh14g13 33:734ce4c36c3f 184 //speed of inner wheel
lh14g13 30:3d6bb5f736a5 185 float vin = speed*(2 - 0.1/tan((theta/0.022222)*(3.14f / 180.0f)));
lh14g13 33:734ce4c36c3f 186 //speed of outer wheel.
lh14g13 30:3d6bb5f736a5 187 float vout = speed*(2 + 0.1/tan((theta/0.022222)*(3.14f / 180.0f)));
lh14g13 27:98aecf1889ed 188 if(leftOrRight){
lh14g13 26:c74e70a745ec 189
lh14g13 26:c74e70a745ec 190 w1=vout;
lh14g13 33:734ce4c36c3f 191
lh14g13 26:c74e70a745ec 192 w2=vin;
lh14g13 26:c74e70a745ec 193
lh14g13 26:c74e70a745ec 194 }
lh14g13 26:c74e70a745ec 195 else{
lh14g13 26:c74e70a745ec 196
lh14g13 26:c74e70a745ec 197 w1=vin;
lh14g13 26:c74e70a745ec 198 w2=vout;
lh14g13 26:c74e70a745ec 199 }
lh14g13 2:cc8ddc587af7 200
lh14g13 26:c74e70a745ec 201 }
lh14g13 14:bc77edc4adb0 202 return;
lh14g13 14:bc77edc4adb0 203 }
lh14g13 2:cc8ddc587af7 204
lh14g13 7:9aaa4f73bb32 205
lh14g13 7:9aaa4f73bb32 206
lh14g13 5:c50e40797114 207
lh14g13 14:bc77edc4adb0 208
lh14g13 14:bc77edc4adb0 209
lh14g13 14:bc77edc4adb0 210
lh14g13 14:bc77edc4adb0 211
lh14g13 15:f40e834d063b 212
lh14g13 15:f40e834d063b 213
lh14g13 15:f40e834d063b 214
lh14g13 15:f40e834d063b 215