GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@21:aaa482dad274, 2016-11-30 (annotated)

Committer:
lh14g13
Date:
Wed Nov 30 11:15:31 2016 +0000
Branch:
motorupdate
Revision:
21:aaa482dad274
Parent:
20:ca87a36c7688
Child:
22:85cf7b6b0422
fuck dat shit

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