GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@15:f40e834d063b, 2016-11-20 (annotated)

Committer:
lh14g13
Date:
Sun Nov 20 13:45:30 2016 +0000
Branch:
motorupdate
Revision:
15:f40e834d063b
Parent:
14:bc77edc4adb0
Child:
16:9c5e9306ae10
cleaned up 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>
lh14g13 0:b0476dcfa14c 4 DigitalOut myled(LED1);
lh14g13 0:b0476dcfa14c 5
lh14g13 7:9aaa4f73bb32 6
lh14g13 0:b0476dcfa14c 7 void TurnOn();
lh14g13 7:9aaa4f73bb32 8 void runMotor();
lh14g13 0:b0476dcfa14c 9 void DefaultMode();
lh14g13 0:b0476dcfa14c 10
lh14g13 0:b0476dcfa14c 11
lh14g13 0:b0476dcfa14c 12 //Speed control
lh14g13 15:f40e834d063b 13
lh14g13 15:f40e834d063b 14 float setDutyCycle(float dutyC, int w,int targetW);
lh14g13 0:b0476dcfa14c 15
lh14g13 0:b0476dcfa14c 16 //Corner Control
lh14g13 0:b0476dcfa14c 17
lh14g13 11:4a6f97cc1f1e 18 void dutyCycleCorner( float speed, float angle);
lh14g13 11:4a6f97cc1f1e 19 void corner(float &w1,float &w2,float deltaTheta,int maxspeed);
lh14g13 0:b0476dcfa14c 20
lh14g13 15:f40e834d063b 21
lh14g13 1:09226806dd15 22 int cornerPwmControl;// this sets the cornering percentage ratio. value between 1 and 0.
lh14g13 0:b0476dcfa14c 23 // 100 means both motors run at the same speed. 0 means that one motor turns off.
lh14g13 0:b0476dcfa14c 24
lh14g13 0:b0476dcfa14c 25
lh14g13 0:b0476dcfa14c 26
lh14g13 0:b0476dcfa14c 27 // uncomment for testing motor functions.
lh14g13 0:b0476dcfa14c 28
lh14g13 0:b0476dcfa14c 29
lh14g13 7:9aaa4f73bb32 30
lh14g13 7:9aaa4f73bb32 31 //need a function for calcu;lating the angle
lh14g13 7:9aaa4f73bb32 32 //need a function for converting w1 to delta. or do i?
lh14g13 0:b0476dcfa14c 33
lh14g13 10:f4fc8ccde4ad 34 //----------------------------------------------------------------------------------------------------------------------------------
lh14g13 10:f4fc8ccde4ad 35 //----------------------------------------------This is for Motor Set up------------------------------------------------------------
lh14g13 10:f4fc8ccde4ad 36 //----------------------------------------------------------------------------------------------------------------------------------
lh14g13 0:b0476dcfa14c 37
lh14g13 7:9aaa4f73bb32 38 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 39 {
lh14g13 0:b0476dcfa14c 40
lh14g13 7:9aaa4f73bb32 41
lh14g13 7:9aaa4f73bb32 42 TFC_SetMotorPWM(0.4,0.7);
lh14g13 0:b0476dcfa14c 43
lh14g13 0:b0476dcfa14c 44 while(1)
lh14g13 0:b0476dcfa14c 45 {
lh14g13 0:b0476dcfa14c 46 if(TFC_ReadPushButton(0)>0)
lh14g13 0:b0476dcfa14c 47 {
lh14g13 0:b0476dcfa14c 48 TFC_SetMotorPWM(0.0,0.0);
lh14g13 7:9aaa4f73bb32 49
lh14g13 0:b0476dcfa14c 50 DefaultMode();
lh14g13 0:b0476dcfa14c 51
lh14g13 0:b0476dcfa14c 52 }
lh14g13 0:b0476dcfa14c 53 }
lh14g13 0:b0476dcfa14c 54 return;
lh14g13 0:b0476dcfa14c 55 }
lh14g13 0:b0476dcfa14c 56
lh14g13 0:b0476dcfa14c 57 void DefaultMode()
lh14g13 0:b0476dcfa14c 58 {
lh14g13 7:9aaa4f73bb32 59 TFC_Init();
lh14g13 0:b0476dcfa14c 60 while(1)
lh14g13 0:b0476dcfa14c 61 {
lh14g13 7:9aaa4f73bb32 62 TFC_HBRIDGE_ENABLE;
lh14g13 0:b0476dcfa14c 63 if(TFC_ReadPushButton(1)>0)
lh14g13 0:b0476dcfa14c 64 {
lh14g13 7:9aaa4f73bb32 65 runMotor();
lh14g13 0:b0476dcfa14c 66 }
lh14g13 0:b0476dcfa14c 67
lh14g13 0:b0476dcfa14c 68 else if(TFC_ReadPushButton(0)>0)
lh14g13 0:b0476dcfa14c 69 {
lh14g13 0:b0476dcfa14c 70 //this will be a debug mode
lh14g13 0:b0476dcfa14c 71 }
lh14g13 0:b0476dcfa14c 72
lh14g13 0:b0476dcfa14c 73
lh14g13 0:b0476dcfa14c 74 }
lh14g13 7:9aaa4f73bb32 75 TFC_HBRIDGE_DISABLE;
lh14g13 0:b0476dcfa14c 76 return;
lh14g13 0:b0476dcfa14c 77 }
lh14g13 10:f4fc8ccde4ad 78 //-----------------------------------------------------------------------------------------------------
lh14g13 10:f4fc8ccde4ad 79 //------------------------ this is for speed control---------------------------------------------------
lh14g13 10:f4fc8ccde4ad 80 //-----------------------------------------------------------------------------------------------------
lh14g13 10:f4fc8ccde4ad 81
lh14g13 15:f40e834d063b 82 float setDutyCycle(float dutyC, int w,int targetW)
lh14g13 15:f40e834d063b 83 {
lh14g13 0:b0476dcfa14c 84
lh14g13 0:b0476dcfa14c 85
lh14g13 15:f40e834d063b 86 if(true)
lh14g13 0:b0476dcfa14c 87 {
lh14g13 15:f40e834d063b 88 if(w<targetW)
lh14g13 15:f40e834d063b 89 {
lh14g13 15:f40e834d063b 90 dutyC+=0.1;
lh14g13 0:b0476dcfa14c 91
lh14g13 15:f40e834d063b 92 }
lh14g13 0:b0476dcfa14c 93
lh14g13 15:f40e834d063b 94 else
lh14g13 15:f40e834d063b 95 {
lh14g13 15:f40e834d063b 96 dutyC-=0.1;
lh14g13 15:f40e834d063b 97 }
lh14g13 0:b0476dcfa14c 98
lh14g13 0:b0476dcfa14c 99 }
lh14g13 0:b0476dcfa14c 100
lh14g13 0:b0476dcfa14c 101
lh14g13 15:f40e834d063b 102 else if(false)
lh14g13 0:b0476dcfa14c 103 {
lh14g13 0:b0476dcfa14c 104
lh14g13 0:b0476dcfa14c 105 }
lh14g13 0:b0476dcfa14c 106
lh14g13 15:f40e834d063b 107
lh14g13 15:f40e834d063b 108 return dutyC;
lh14g13 9:22b119eef1de 109 }
lh14g13 9:22b119eef1de 110
lh14g13 15:f40e834d063b 111
lh14g13 8:4df2a47ab4ee 112 //----------------------------------------------------------------------------------------------------------------------------
lh14g13 8:4df2a47ab4ee 113 //------------------------------------------------Cornering Control-----------------------------------------------------------
lh14g13 8:4df2a47ab4ee 114 //----------------------------------------------------------------------------------------------------------------------------
lh14g13 2:cc8ddc587af7 115
lh14g13 10:f4fc8ccde4ad 116
lh14g13 10:f4fc8ccde4ad 117
lh14g13 13:c43f157a6bac 118 // tThis is a function which works off of the duty cycle. NO SENSOR REQUIREMENT
lh14g13 13:c43f157a6bac 119 // CLEAN THIS UP AND UPDATE FOR SENSORS VERSION WITH THIS CODE
lh14g13 14:bc77edc4adb0 120 // 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 121 void dutyCycleCorner( float speed, float theta)
lh14g13 10:f4fc8ccde4ad 122 {
lh14g13 11:4a6f97cc1f1e 123 bool leftOrRight;
lh14g13 11:4a6f97cc1f1e 124 if(theta<0)
lh14g13 11:4a6f97cc1f1e 125 {
lh14g13 14:bc77edc4adb0 126 leftOrRight=true;
lh14g13 14:bc77edc4adb0 127 theta=theta*-1;
lh14g13 11:4a6f97cc1f1e 128
lh14g13 11:4a6f97cc1f1e 129 }
lh14g13 14:bc77edc4adb0 130
lh14g13 15:f40e834d063b 131 float deltaW = ((0.2f*tan((theta/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed;
lh14g13 11:4a6f97cc1f1e 132
lh14g13 14:bc77edc4adb0 133 //TFC_SetMotorPWM(w2,w1);
lh14g13 14:bc77edc4adb0 134 if(leftOrRight){
lh14g13 14:bc77edc4adb0 135 TFC_SetMotorPWM(speed+ deltaW,speed- deltaW);
lh14g13 11:4a6f97cc1f1e 136 }
lh14g13 11:4a6f97cc1f1e 137 else{
lh14g13 14:bc77edc4adb0 138 TFC_SetMotorPWM(speed- deltaW,speed+ deltaW);
lh14g13 14:bc77edc4adb0 139 }
lh14g13 10:f4fc8ccde4ad 140
lh14g13 10:f4fc8ccde4ad 141
lh14g13 10:f4fc8ccde4ad 142
lh14g13 10:f4fc8ccde4ad 143 return;
lh14g13 10:f4fc8ccde4ad 144 }
lh14g13 10:f4fc8ccde4ad 145
lh14g13 2:cc8ddc587af7 146
lh14g13 15:f40e834d063b 147 void sensorCorner(float &w1,float &w2,float theta,int speed)
lh14g13 14:bc77edc4adb0 148 { // when cornering left the left motor slows down more than the right hand side
lh14g13 2:cc8ddc587af7 149 // may just replace with ACC and DECC
lh14g13 14:bc77edc4adb0 150
lh14g13 14:bc77edc4adb0 151 bool leftOrRight;
lh14g13 11:4a6f97cc1f1e 152
lh14g13 14:bc77edc4adb0 153 if(theta<0)
lh14g13 14:bc77edc4adb0 154 {
lh14g13 14:bc77edc4adb0 155 leftOrRight=true;
lh14g13 14:bc77edc4adb0 156 theta=theta*-1;
lh14g13 14:bc77edc4adb0 157
lh14g13 14:bc77edc4adb0 158 }
lh14g13 14:bc77edc4adb0 159
lh14g13 15:f40e834d063b 160 float deltaW = ((0.2f*tan((theta/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed;
lh14g13 3:5b5d5af46804 161
lh14g13 15:f40e834d063b 162 w1= speed+ deltaW;
lh14g13 15:f40e834d063b 163 w2= speed -deltaW;
lh14g13 2:cc8ddc587af7 164
lh14g13 2:cc8ddc587af7 165
lh14g13 14:bc77edc4adb0 166 return;
lh14g13 14:bc77edc4adb0 167 }
lh14g13 2:cc8ddc587af7 168
lh14g13 7:9aaa4f73bb32 169
lh14g13 7:9aaa4f73bb32 170
lh14g13 5:c50e40797114 171
lh14g13 14:bc77edc4adb0 172
lh14g13 14:bc77edc4adb0 173
lh14g13 14:bc77edc4adb0 174
lh14g13 14:bc77edc4adb0 175
lh14g13 15:f40e834d063b 176
lh14g13 15:f40e834d063b 177
lh14g13 15:f40e834d063b 178
lh14g13 15:f40e834d063b 179