GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@27:98aecf1889ed, 2017-01-11 (annotated)

Committer:
lh14g13
Date:
Wed Jan 11 11:17:25 2017 +0000
Branch:
motorupdate
Revision:
27:98aecf1889ed
Parent:
26:c74e70a745ec
Child:
28:9d4042b05640
added comments

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 11:4a6f97cc1f1e 114 if(theta<0)
lh14g13 11:4a6f97cc1f1e 115 {
lh14g13 14:bc77edc4adb0 116 leftOrRight=true;
lh14g13 14:bc77edc4adb0 117 theta=theta*-1;
lh14g13 11:4a6f97cc1f1e 118
lh14g13 11:4a6f97cc1f1e 119 }
lh14g13 14:bc77edc4adb0 120
lh14g13 18:e3fd26490f58 121 float deltaW = ((0.1f*tan((theta/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed;
lh14g13 11:4a6f97cc1f1e 122
lh14g13 14:bc77edc4adb0 123 //TFC_SetMotorPWM(w2,w1);
lh14g13 14:bc77edc4adb0 124 if(leftOrRight){
lh14g13 14:bc77edc4adb0 125 TFC_SetMotorPWM(speed+ deltaW,speed- deltaW);
lh14g13 11:4a6f97cc1f1e 126 }
lh14g13 11:4a6f97cc1f1e 127 else{
lh14g13 14:bc77edc4adb0 128 TFC_SetMotorPWM(speed- deltaW,speed+ deltaW);
lh14g13 14:bc77edc4adb0 129 }
lh14g13 10:f4fc8ccde4ad 130
lh14g13 10:f4fc8ccde4ad 131
lh14g13 10:f4fc8ccde4ad 132
lh14g13 10:f4fc8ccde4ad 133 return;
lh14g13 10:f4fc8ccde4ad 134 }
lh14g13 10:f4fc8ccde4ad 135
lh14g13 2:cc8ddc587af7 136
lh14g13 23:96c36a38d83b 137 void sensorCorner(float &w1,float &w2,float theta,float speed)
lh14g13 14:bc77edc4adb0 138 { // when cornering left the left motor slows down more than the right hand side
lh14g13 27:98aecf1889ed 139 // this is the ED for when the car is running off of the sensors rather than a set PWM
lh14g13 14:bc77edc4adb0 140
lh14g13 24:15c6bbdbb0e4 141 bool leftOrRight = false;
lh14g13 24:15c6bbdbb0e4 142 float tune = 1.2;
lh14g13 14:bc77edc4adb0 143 if(theta<0)
lh14g13 14:bc77edc4adb0 144 {
lh14g13 14:bc77edc4adb0 145 leftOrRight=true;
lh14g13 14:bc77edc4adb0 146 theta=theta*-1;
lh14g13 14:bc77edc4adb0 147
lh14g13 14:bc77edc4adb0 148 }
lh14g13 24:15c6bbdbb0e4 149
lh14g13 24:15c6bbdbb0e4 150 if(theta>0.5)
lh14g13 24:15c6bbdbb0e4 151 {
lh14g13 24:15c6bbdbb0e4 152 theta = 0.5;
lh14g13 24:15c6bbdbb0e4 153
lh14g13 24:15c6bbdbb0e4 154 }
lh14g13 27:98aecf1889ed 155
lh14g13 27:98aecf1889ed 156
lh14g13 27:98aecf1889ed 157
lh14g13 3:5b5d5af46804 158
lh14g13 27:98aecf1889ed 159 //These equations set the angular speeds of the motors
lh14g13 27:98aecf1889ed 160 //there are two equations for testing purposes
lh14g13 26:c74e70a745ec 161 if(true){
lh14g13 27:98aecf1889ed 162 float deltaW = ((0.1f*tan((theta/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed;
lh14g13 20:ca87a36c7688 163 if(leftOrRight){
lh14g13 24:15c6bbdbb0e4 164 w1= speed+ deltaW * tune;
lh14g13 24:15c6bbdbb0e4 165 w2= speed -deltaW * tune;
lh14g13 27:98aecf1889ed 166
lh14g13 25:255d169a45e1 167
lh14g13 21:aaa482dad274 168 }
lh14g13 21:aaa482dad274 169 else{
lh14g13 24:15c6bbdbb0e4 170 w1= speed- deltaW*tune;
lh14g13 24:15c6bbdbb0e4 171 w2= speed +deltaW*tune;
lh14g13 27:98aecf1889ed 172
lh14g13 20:ca87a36c7688 173 }
lh14g13 26:c74e70a745ec 174 }
lh14g13 27:98aecf1889ed 175
lh14g13 26:c74e70a745ec 176 else{
lh14g13 27:98aecf1889ed 177 float vin = speed(2 - 0.1/tan((theta/0.022222)*(3.14f / 180.0f));
lh14g13 27:98aecf1889ed 178 float vout = speed(2 + 0.1/tan((theta/0.022222)*(3.14f / 180.0f));
lh14g13 27:98aecf1889ed 179 if(leftOrRight){
lh14g13 26:c74e70a745ec 180
lh14g13 26:c74e70a745ec 181 w1=vout;
lh14g13 26:c74e70a745ec 182 w2=vin;
lh14g13 26:c74e70a745ec 183
lh14g13 26:c74e70a745ec 184 }
lh14g13 26:c74e70a745ec 185 else{
lh14g13 26:c74e70a745ec 186
lh14g13 26:c74e70a745ec 187 w1=vin;
lh14g13 26:c74e70a745ec 188 w2=vout;
lh14g13 26:c74e70a745ec 189 }
lh14g13 2:cc8ddc587af7 190
lh14g13 26:c74e70a745ec 191 }
lh14g13 14:bc77edc4adb0 192 return;
lh14g13 14:bc77edc4adb0 193 }
lh14g13 2:cc8ddc587af7 194
lh14g13 7:9aaa4f73bb32 195
lh14g13 7:9aaa4f73bb32 196
lh14g13 5:c50e40797114 197
lh14g13 14:bc77edc4adb0 198
lh14g13 14:bc77edc4adb0 199
lh14g13 14:bc77edc4adb0 200
lh14g13 14:bc77edc4adb0 201
lh14g13 15:f40e834d063b 202
lh14g13 15:f40e834d063b 203
lh14g13 15:f40e834d063b 204
lh14g13 15:f40e834d063b 205