GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@25:255d169a45e1, 2017-01-11 (annotated)

Committer:
lh14g13
Date:
Wed Jan 11 11:06:18 2017 +0000
Branch:
motorupdate
Revision:
25:255d169a45e1
Parent:
24:15c6bbdbb0e4
Child:
26:c74e70a745ec
new ED equation. comment in for testing the difference

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 19:69c142d81437 7 //adam sucks
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 7:9aaa4f73bb32 27 void runMotor() /// putting it into this mode for some reason makes a bit of a whinning noise (this may simply just be the motor running)
lh14g13 0:b0476dcfa14c 28 {
lh14g13 0:b0476dcfa14c 29
lh14g13 7:9aaa4f73bb32 30
lh14g13 7:9aaa4f73bb32 31 TFC_SetMotorPWM(0.4,0.7);
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 0:b0476dcfa14c 45
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 10:f4fc8ccde4ad 70
lh14g13 15:f40e834d063b 71 float setDutyCycle(float dutyC, int w,int targetW)
lh14g13 15:f40e834d063b 72 {
lh14g13 0:b0476dcfa14c 73
lh14g13 0:b0476dcfa14c 74
lh14g13 15:f40e834d063b 75 if(true)
lh14g13 0:b0476dcfa14c 76 {
lh14g13 15:f40e834d063b 77 if(w<targetW)
lh14g13 15:f40e834d063b 78 {
lh14g13 15:f40e834d063b 79 dutyC+=0.1;
lh14g13 0:b0476dcfa14c 80
lh14g13 15:f40e834d063b 81 }
lh14g13 0:b0476dcfa14c 82
lh14g13 15:f40e834d063b 83 else
lh14g13 15:f40e834d063b 84 {
lh14g13 15:f40e834d063b 85 dutyC-=0.1;
lh14g13 15:f40e834d063b 86 }
lh14g13 0:b0476dcfa14c 87
lh14g13 0:b0476dcfa14c 88 }
lh14g13 0:b0476dcfa14c 89
lh14g13 0:b0476dcfa14c 90
lh14g13 15:f40e834d063b 91 else if(false)
lh14g13 0:b0476dcfa14c 92 {
lh14g13 0:b0476dcfa14c 93
lh14g13 0:b0476dcfa14c 94 }
lh14g13 0:b0476dcfa14c 95
lh14g13 15:f40e834d063b 96
lh14g13 15:f40e834d063b 97 return dutyC;
lh14g13 9:22b119eef1de 98 }
lh14g13 9:22b119eef1de 99
lh14g13 15:f40e834d063b 100
lh14g13 8:4df2a47ab4ee 101 //----------------------------------------------------------------------------------------------------------------------------
lh14g13 8:4df2a47ab4ee 102 //------------------------------------------------Cornering Control-----------------------------------------------------------
lh14g13 8:4df2a47ab4ee 103 //----------------------------------------------------------------------------------------------------------------------------
lh14g13 2:cc8ddc587af7 104
lh14g13 10:f4fc8ccde4ad 105
lh14g13 10:f4fc8ccde4ad 106
lh14g13 13:c43f157a6bac 107 // tThis is a function which works off of the duty cycle. NO SENSOR REQUIREMENT
lh14g13 13:c43f157a6bac 108 // CLEAN THIS UP AND UPDATE FOR SENSORS VERSION WITH THIS CODE
lh14g13 14:bc77edc4adb0 109 // 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 110 void dutyCycleCorner( float speed, float theta)
lh14g13 10:f4fc8ccde4ad 111 {
lh14g13 11:4a6f97cc1f1e 112 bool leftOrRight;
lh14g13 11:4a6f97cc1f1e 113 if(theta<0)
lh14g13 11:4a6f97cc1f1e 114 {
lh14g13 14:bc77edc4adb0 115 leftOrRight=true;
lh14g13 14:bc77edc4adb0 116 theta=theta*-1;
lh14g13 11:4a6f97cc1f1e 117
lh14g13 11:4a6f97cc1f1e 118 }
lh14g13 14:bc77edc4adb0 119
lh14g13 18:e3fd26490f58 120 float deltaW = ((0.1f*tan((theta/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed;
lh14g13 11:4a6f97cc1f1e 121
lh14g13 14:bc77edc4adb0 122 //TFC_SetMotorPWM(w2,w1);
lh14g13 14:bc77edc4adb0 123 if(leftOrRight){
lh14g13 14:bc77edc4adb0 124 TFC_SetMotorPWM(speed+ deltaW,speed- deltaW);
lh14g13 11:4a6f97cc1f1e 125 }
lh14g13 11:4a6f97cc1f1e 126 else{
lh14g13 14:bc77edc4adb0 127 TFC_SetMotorPWM(speed- deltaW,speed+ deltaW);
lh14g13 14:bc77edc4adb0 128 }
lh14g13 10:f4fc8ccde4ad 129
lh14g13 10:f4fc8ccde4ad 130
lh14g13 10:f4fc8ccde4ad 131
lh14g13 10:f4fc8ccde4ad 132 return;
lh14g13 10:f4fc8ccde4ad 133 }
lh14g13 10:f4fc8ccde4ad 134
lh14g13 2:cc8ddc587af7 135
lh14g13 23:96c36a38d83b 136 void sensorCorner(float &w1,float &w2,float theta,float speed)
lh14g13 14:bc77edc4adb0 137 { // when cornering left the left motor slows down more than the right hand side
lh14g13 24:15c6bbdbb0e4 138
lh14g13 14:bc77edc4adb0 139
lh14g13 24:15c6bbdbb0e4 140 bool leftOrRight = false;
lh14g13 24:15c6bbdbb0e4 141 float tune = 1.2;
lh14g13 14:bc77edc4adb0 142 if(theta<0)
lh14g13 14:bc77edc4adb0 143 {
lh14g13 14:bc77edc4adb0 144 leftOrRight=true;
lh14g13 14:bc77edc4adb0 145 theta=theta*-1;
lh14g13 14:bc77edc4adb0 146
lh14g13 14:bc77edc4adb0 147 }
lh14g13 24:15c6bbdbb0e4 148
lh14g13 24:15c6bbdbb0e4 149 if(theta>0.5)
lh14g13 24:15c6bbdbb0e4 150 {
lh14g13 24:15c6bbdbb0e4 151 theta = 0.5;
lh14g13 24:15c6bbdbb0e4 152
lh14g13 24:15c6bbdbb0e4 153 }
lh14g13 18:e3fd26490f58 154 float deltaW = ((0.1f*tan((theta/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed;
lh14g13 3:5b5d5af46804 155
lh14g13 25:255d169a45e1 156 //float vin = speed(2 - 0.1/tan((theta/0.022222)*(3.14f / 180.0f));
lh14g13 25:255d169a45e1 157 //float vout = speed(2 + 0.1/tan((theta/0.022222)*(3.14f / 180.0f));
lh14g13 25:255d169a45e1 158
lh14g13 20:ca87a36c7688 159 if(leftOrRight){
lh14g13 24:15c6bbdbb0e4 160 w1= speed+ deltaW * tune;
lh14g13 24:15c6bbdbb0e4 161 w2= speed -deltaW * tune;
lh14g13 25:255d169a45e1 162 //w1=vout;
lh14g13 25:255d169a45e1 163 //w2=vin;
lh14g13 25:255d169a45e1 164
lh14g13 21:aaa482dad274 165 }
lh14g13 21:aaa482dad274 166 else{
lh14g13 24:15c6bbdbb0e4 167 w1= speed- deltaW*tune;
lh14g13 24:15c6bbdbb0e4 168 w2= speed +deltaW*tune;
lh14g13 25:255d169a45e1 169 //w1=vin;
lh14g13 25:255d169a45e1 170 //w2=vout;
lh14g13 20:ca87a36c7688 171 }
lh14g13 20:ca87a36c7688 172
lh14g13 2:cc8ddc587af7 173
lh14g13 2:cc8ddc587af7 174
lh14g13 14:bc77edc4adb0 175 return;
lh14g13 14:bc77edc4adb0 176 }
lh14g13 2:cc8ddc587af7 177
lh14g13 7:9aaa4f73bb32 178
lh14g13 7:9aaa4f73bb32 179
lh14g13 5:c50e40797114 180
lh14g13 14:bc77edc4adb0 181
lh14g13 14:bc77edc4adb0 182
lh14g13 14:bc77edc4adb0 183
lh14g13 14:bc77edc4adb0 184
lh14g13 15:f40e834d063b 185
lh14g13 15:f40e834d063b 186
lh14g13 15:f40e834d063b 187
lh14g13 15:f40e834d063b 188