GDP 4
changes to motor library
Fork of
motor
by 
Lawrence Harlow
Diff: motor.cpp
- Revision:
- 17:28216063e33e
- Parent:
- 5:c50e40797114
--- a/motor.cpp Mon Oct 24 14:44:12 2016 +0000
+++ b/motor.cpp Tue Nov 29 13:11:06 2016 +0000
@@ -1,57 +1,51 @@
#include "mbed.h"
#include "TFC.h"
+#include <math.h>
DigitalOut myled(LED1);
-int motor_setup();
+
void TurnOn();
-void RunMotor();
+void runMotor();
void DefaultMode();
+//Speed control
-//Speed control
-void Acc(float& motorA, float& motorB);
-void Decc(float& motorA, float& motorB);
-void StartLine();
+float setDutyCycle(float dutyC, int w,int targetW);
//Corner Control
-void PWM_cornerCntrl(bool a,float pwmRatio);
-void cornerLeft(float speed);
-void cornerRight(float speed);
+void dutyCycleCorner( float speed, float angle);
+void corner(float &w1,float &w2,float deltaTheta,int maxspeed);
-int cornerPwmControl;// this sets the cornering percentage ratio. value between 1 and 0.
- // 100 means both motors run at the same speed. 0 means that one motor turns off.
+//Variables
+int cornerPwmControl;
// uncomment for testing motor functions.
- int motor_setup()
+
+//need a function for calcu;lating the angle
+//need a function for converting w1 to delta. or do i?
+
+//----------------------------------------------------------------------------------------------------------------------------------
+//----------------------------------------------This is for Motor Set up------------------------------------------------------------
+//----------------------------------------------------------------------------------------------------------------------------------
+
+ 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)
{
- //TurnOn();
- return 0;
- }
-
-
-
-
-
- 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)
- {
-
- TFC_HBRIDGE_ENABLE;
- // TFC_SetMotorPWM(0.4,0.7);
+ TFC_SetMotorPWM(0.4,0.7);
while(1)
{
if(TFC_ReadPushButton(0)>0)
{
TFC_SetMotorPWM(0.0,0.0);
- TFC_HBRIDGE_DISABLE;
+
DefaultMode();
}
@@ -61,11 +55,13 @@
void DefaultMode()
{
+ TFC_Init();
while(1)
{
+ TFC_HBRIDGE_ENABLE;
if(TFC_ReadPushButton(1)>0)
{
- RunMotor();
+ runMotor();
}
else if(TFC_ReadPushButton(0)>0)
@@ -75,153 +71,108 @@
}
+ TFC_HBRIDGE_DISABLE;
return;
}
+//-----------------------------------------------------------------------------------------------------
+//------------------------ this is for speed control---------------------------------------------------
+//-----------------------------------------------------------------------------------------------------
+
+float setDutyCycle(float dutyC, int w,int targetW)
+{
-void Acc(float& motorA, float& motorB)// set up so as to control both motors during acc. Potential use during corners
+ if(true)
{
- motorA = motorA + 0.1;
- motorB = motorB + 0.1;
- TFC_SetMotorPWM(motorA,motorB);
+ if(w<targetW)
+ {
+ dutyC+=0.1;
-
- return ;
-
- }
-
-void Decc(float &motorA, float &motorB)
- {
- // a good thing to do would be to have a margin for adjustment so that the car cornering can control the acc+dcc much better.
+ }
- motorA = motorA - 0.1;
- motorB = motorB - 0.1;
+ else
+ {
+ dutyC-=0.1;
+ }
- TFC_SetMotorPWM(motorA,motorB);
-
- return ;
-
}
-void StartLine()
+ else if(false)
{
- TFC_HBRIDGE_ENABLE;
- float a=0;
- float b=0;
-
- int x=0 ;
- while(x<5)
- {
- Acc(a,b);
- wait(0.5);
- x++ ;
- }
- return ;
}
-void finishLine(float pwmA)
- {
- float pwmB= pwmA;
- while(pwmA>0)
- {
-
-
- Decc(pwmA,pwmB);
-
- }
- return;
- }
+
+ return dutyC;
+}
+
-void PWM_cornerCntrl(bool a,float pwmRatio)
- {
- //A is the Right motor B is the left motor.
- // may need the value of speed to decelerate.
- if(a==1)//turn left
- {
- cornerLeft(pwmRatio);
- }
-
- else//turn right
- {
- cornerRight(pwmRatio);
- }
- return;
- }
-
+ //----------------------------------------------------------------------------------------------------------------------------
+ //------------------------------------------------Cornering Control-----------------------------------------------------------
+ //----------------------------------------------------------------------------------------------------------------------------
-void cornerLeft(float speed)
- {// when cornering left the left motor slows down more than the right hand side
- // may just replace with ACC and DECC
-
- // it may be worth doing this off the change in theta. so that it is insesnsitive to the calibration.
- float diff = speed*cornerPwmControl;
- float w1 = speed + diff;
- float w2 = speed-diff;
- TFC_SetMotorPWM(w2,w1); //temperary values
+// tThis is a function which works off of the duty cycle. NO SENSOR REQUIREMENT
+// CLEAN THIS UP AND UPDATE FOR SENSORS VERSION WITH THIS CODE
+// 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.
+void dutyCycleCorner( float speed, float theta)
+{
+ bool leftOrRight;
+ if(theta<0)
+ {
+ leftOrRight=true;
+ theta=theta*-1;
+
+ }
+
+ float deltaW = ((0.2f*tan((theta/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed;
-
-
-
+ //TFC_SetMotorPWM(w2,w1);
+ if(leftOrRight){
+ TFC_SetMotorPWM(speed+ deltaW,speed- deltaW);
+ }
+ else{
+ TFC_SetMotorPWM(speed- deltaW,speed+ deltaW);
+ }
+
+
-
-
- return;
- }
+ return;
+}
+
+
+void sensorCorner(float &w1,float &w2,float theta,int speed)
+{ // when cornering left the left motor slows down more than the right hand side
+ // may just replace with ACC and DECC
+
+ bool leftOrRight;
-void cornerRight(float speed)
- { // may need to put deceleration control within here.
- // may just replace with ACC and DECC
- float diff = speed*cornerPwmControl;
- float w1 = speed + diff;
- float w2 = speed-diff;
- TFC_SetMotorPWM(w1,w2); //temperary values
+ if(theta<0)
+ {
+ leftOrRight=true;
+ theta=theta*-1;
+
+ }
+
+ float deltaW = ((0.2f*tan((theta/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed;
+
+ w1= speed+ deltaW;
+ w2= speed -deltaW;
- return;
- }
+
+ return;
+}
+
+
-void deltaCornerLeft(float speed,float deltaTheta)
- {// when cornering left the left motor slows down more than the right hand side
- // may just replace with ACC and DECC
- float r;
- float d;
- float l;
- // it may be worth doing this off the change in theta. so that it is insesnsitive to the calibration.
- float diff= ((d*tan(deltaTheta)/(2*l)));
-
- float w1 = (speed/r)*(1+diff);
- float w2 = (speed/r)*(1-diff);
-
- // when there is a speed sensor tghe conversion will be much more simplistic. this is basically just guessing.
- // need to convert w1 to the duty cycle
- TFC_SetMotorPWM(w2,w1); //temperary values
-
-
- return;
- }
-
-void deltaCornerRight(float speed,float deltaTheta)
- {// when cornering right the inner motor slows down more than the right hand side
- // may just replace with ACC and DECC
- float r;
- float d;
- float l;
- // it may be worth doing this off the change in theta. so that it is insesnsitive to the calibration.
- float diff= ((d*tan(deltaTheta)/(2*l)));
-
- float w1 = (speed/r)*(1+diff);
- float w2 = (speed/r)*(1-diff);
-
- // when there is a speed sensor tghe conversion will be much more simplistic. this is basically just guessing.
- // need to convert w1 to the duty cycle
- TFC_SetMotorPWM(w2,w1); //temperary values
-
-
- return;
- }
-
-
+
+
+
+
+
+
+
+