GDP 4 / motor2

changes to motor library

Dependents:   pid-car-example

Fork of motor by Lawrence Harlow

motor.cpp@17:28216063e33e, 2016-11-29 (annotated)

Committer:
FatCookies
Date:
Tue Nov 29 13:11:06 2016 +0000
Revision:
17:28216063e33e
Parent:
5:c50e40797114 
i must have changed something

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