302-2019 Group 2 / Mbed 2 deprecated 302CombinedCode

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@3:b17ae88bfa54, 2019-03-27 (annotated)

Committer:
Jtroper
Date:
Wed Mar 27 17:24:22 2019 +0000
Revision:
3:b17ae88bfa54
Parent:
2:a17f7da1ca7c
Child:
4:11559b04c4ff
Combined with state logic

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jtroper 0:0e723924ae7c 1 #include "mbed.h"
Jtroper 1:b86f099a314a 2 #include <iostream>
Jtroper 2:a17f7da1ca7c 3 #define TI .001f //1kHz sample time
Jtroper 3:b17ae88bfa54 4 #define dKP 5.1491f //Proportional Gain
Jtroper 3:b17ae88bfa54 5 #define dKI 119.3089f //Integral Gain
Jtroper 3:b17ae88bfa54 6 #define FPWM .00004f //PWM Frequency#define TI 0.001 //1kHz sample time
Jtroper 3:b17ae88bfa54 7 #define sKP 3.50f //Proportional Gain
Jtroper 3:b17ae88bfa54 8 #define sKD 0.000f //Derivative Gain was .01
Jtroper 3:b17ae88bfa54 9 #define DCINTERCEPT 0.061f //duty cycle intercept
Jtroper 3:b17ae88bfa54 10 #define DCSLOPE 0.05f //duty cycle slope
Jtroper 3:b17ae88bfa54 11 #define TOL 0.005f //tolerance
Jtroper 3:b17ae88bfa54 12 #ifndef M_PI
Jtroper 3:b17ae88bfa54 13 #define M_PI 3.14f
Jtroper 3:b17ae88bfa54 14 #endif
Jtroper 2:a17f7da1ca7c 15
Jtroper 2:a17f7da1ca7c 16 //Pins
Jtroper 1:b86f099a314a 17 DigitalOut myRled(LED1);
Jtroper 1:b86f099a314a 18 DigitalOut myBled(LED2);
Jtroper 3:b17ae88bfa54 19 DigitalOut myBled(LED2);
Jtroper 3:b17ae88bfa54 20
Jtroper 3:b17ae88bfa54 21 AnalogIn _SpeedReference(PTB0);
Jtroper 2:a17f7da1ca7c 22 AnalogIn _Tacho(PTB2);
Jtroper 2:a17f7da1ca7c 23 PwmOut _MotorControl(PTB1);
Jtroper 3:b17ae88bfa54 24 DigitalOut _BreakSignal(PTB3);
Jtroper 3:b17ae88bfa54 25
Jtroper 3:b17ae88bfa54 26 AnalogIn _LeftSensors(A0);
Jtroper 3:b17ae88bfa54 27 AnalogIn _RightSensors(A1);
Jtroper 3:b17ae88bfa54 28 //AnalogIn _ReferenceSignal(A2);
Jtroper 3:b17ae88bfa54 29 PwmOut _servo(PTE20);
Jtroper 2:a17f7da1ca7c 30
Jtroper 2:a17f7da1ca7c 31 Ticker Update;
Jtroper 2:a17f7da1ca7c 32
Jtroper 2:a17f7da1ca7c 33 //Integral Value
Jtroper 2:a17f7da1ca7c 34 float _errorArea;
Jtroper 2:a17f7da1ca7c 35 float _error;
Jtroper 2:a17f7da1ca7c 36 float _controllerOutput;
Jtroper 2:a17f7da1ca7c 37 float _ref;
Jtroper 3:b17ae88bfa54 38 float _feedbackPrev;
Jtroper 3:b17ae88bfa54 39
Jtroper 3:b17ae88bfa54 40 //States
Jtroper 3:b17ae88bfa54 41 enum States {RUN, WAIT, STOP, EMPTY};
Jtroper 3:b17ae88bfa54 42 States _State;
Jtroper 2:a17f7da1ca7c 43
Jtroper 2:a17f7da1ca7c 44 //Methods
Jtroper 2:a17f7da1ca7c 45 void Init();
Jtroper 2:a17f7da1ca7c 46 void Loop();
Jtroper 2:a17f7da1ca7c 47 void UpdateMethod();
Jtroper 3:b17ae88bfa54 48 void UpdateDriveControl();
Jtroper 3:b17ae88bfa54 49 void UpdateSteeringControl();
Jtroper 2:a17f7da1ca7c 50 void SetMotor(float);
Jtroper 3:b17ae88bfa54 51 void BreaksOn();
Jtroper 3:b17ae88bfa54 52 void BreaksOff();
Jtroper 3:b17ae88bfa54 53
Jtroper 3:b17ae88bfa54 54 //State Methods
Jtroper 3:b17ae88bfa54 55 void WaitState();
Jtroper 3:b17ae88bfa54 56 void RunState();
Jtroper 3:b17ae88bfa54 57 void StopState();
Jtroper 0:0e723924ae7c 58
Jtroper 0:0e723924ae7c 59 int main() {
Jtroper 2:a17f7da1ca7c 60 Init();
Jtroper 2:a17f7da1ca7c 61 while(1) {Loop();}
Jtroper 2:a17f7da1ca7c 62 }
Jtroper 2:a17f7da1ca7c 63
Jtroper 2:a17f7da1ca7c 64 void Init() //Initializer
Jtroper 2:a17f7da1ca7c 65 {
Jtroper 2:a17f7da1ca7c 66 _MotorControl.period(FPWM);
Jtroper 2:a17f7da1ca7c 67 SetMotor(0.0f); //Turns off the motor
Jtroper 2:a17f7da1ca7c 68 _error = 0.0f;
Jtroper 2:a17f7da1ca7c 69 _errorArea = 0.0f;
Jtroper 2:a17f7da1ca7c 70 _ref = 0.0f;
Jtroper 3:b17ae88bfa54 71 _State = STOP;
Jtroper 3:b17ae88bfa54 72 _feedbackPrev = 0.0f;
Jtroper 3:b17ae88bfa54 73 _servo.period(0.02);
Jtroper 3:b17ae88bfa54 74 BreaksOff();
Jtroper 2:a17f7da1ca7c 75 Update.attach(&UpdateMethod, TI);
Jtroper 2:a17f7da1ca7c 76 //SetMotor(_ref);
Jtroper 2:a17f7da1ca7c 77 }
Jtroper 2:a17f7da1ca7c 78
Jtroper 2:a17f7da1ca7c 79 void Loop() //Repeated Loop
Jtroper 2:a17f7da1ca7c 80 {
Jtroper 2:a17f7da1ca7c 81 wait(5);
Jtroper 2:a17f7da1ca7c 82 cout<< _ref << endl;
Jtroper 0:0e723924ae7c 83 }
Jtroper 2:a17f7da1ca7c 84
Jtroper 2:a17f7da1ca7c 85 void UpdateMethod() //
Jtroper 2:a17f7da1ca7c 86 {
Jtroper 3:b17ae88bfa54 87 UpdateDriveControl();
Jtroper 3:b17ae88bfa54 88 UpdateSteeringControl();
Jtroper 2:a17f7da1ca7c 89 }
Jtroper 2:a17f7da1ca7c 90
Jtroper 3:b17ae88bfa54 91 void UpdateDriveControl()
Jtroper 2:a17f7da1ca7c 92 {
Jtroper 3:b17ae88bfa54 93 if(_State == RUN)
Jtroper 3:b17ae88bfa54 94 {
Jtroper 3:b17ae88bfa54 95 _ref = _SpeedReference.read();
Jtroper 2:a17f7da1ca7c 96 _error = _ref - _Tacho.read();
Jtroper 2:a17f7da1ca7c 97 _errorArea += TI*_error;
Jtroper 3:b17ae88bfa54 98 _controllerOutput = dKP*_error+ dKI*_errorArea;
Jtroper 2:a17f7da1ca7c 99 SetMotor(_controllerOutput);
Jtroper 3:b17ae88bfa54 100 }
Jtroper 2:a17f7da1ca7c 101 }
Jtroper 2:a17f7da1ca7c 102
Jtroper 3:b17ae88bfa54 103 void UpdateSteeringControl(void)
Jtroper 3:b17ae88bfa54 104 {
Jtroper 3:b17ae88bfa54 105 //float feedbackPrev = 0;
Jtroper 3:b17ae88bfa54 106 float feedback = _LeftSensors.read() - _RightSensors.read();
Jtroper 3:b17ae88bfa54 107 float reference = 0.0f;//_ReferenceSignal.read();
Jtroper 3:b17ae88bfa54 108 float err = reference - feedback;
Jtroper 3:b17ae88bfa54 109 float feedbackChange = (0.0f/*feedback*/ - _feedbackPrev)/TI;
Jtroper 3:b17ae88bfa54 110 float controllerOut = sKP*err + sKD*feedbackChange;
Jtroper 3:b17ae88bfa54 111 float controllerOutNorm = (controllerOut + M_PI/2.0f)/M_PI;
Jtroper 3:b17ae88bfa54 112 float dutyCycle = DCSLOPE*controllerOutNorm + DCINTERCEPT;
Jtroper 3:b17ae88bfa54 113 if (abs(dutyCycle-_servo.read()) > TOL)
Jtroper 3:b17ae88bfa54 114 _servo.write(dutyCycle);
Jtroper 3:b17ae88bfa54 115 _feedbackPrev = feedback;
Jtroper 3:b17ae88bfa54 116
Jtroper 3:b17ae88bfa54 117 }
Jtroper 2:a17f7da1ca7c 118 void SetMotor(float speed)
Jtroper 2:a17f7da1ca7c 119 {
Jtroper 2:a17f7da1ca7c 120 float invD = 0.0f;
Jtroper 2:a17f7da1ca7c 121 if(speed<0.0f)
Jtroper 2:a17f7da1ca7c 122 invD =0.0f;
Jtroper 2:a17f7da1ca7c 123 else if(speed> .75f)
Jtroper 2:a17f7da1ca7c 124 invD = .75f;
Jtroper 2:a17f7da1ca7c 125 else
Jtroper 2:a17f7da1ca7c 126 invD = speed;
Jtroper 2:a17f7da1ca7c 127
Jtroper 2:a17f7da1ca7c 128 _MotorControl.write(/*1.0f-*/invD);
Jtroper 3:b17ae88bfa54 129 }
Jtroper 3:b17ae88bfa54 130
Jtroper 3:b17ae88bfa54 131 void BreaksOn()
Jtroper 3:b17ae88bfa54 132 {
Jtroper 3:b17ae88bfa54 133 _controllerOutput = 0;
Jtroper 3:b17ae88bfa54 134 _errorArea = 0;
Jtroper 3:b17ae88bfa54 135 _BreakSignal = 0;
Jtroper 3:b17ae88bfa54 136 }
Jtroper 3:b17ae88bfa54 137
Jtroper 3:b17ae88bfa54 138 void BreaksOff()
Jtroper 3:b17ae88bfa54 139 {
Jtroper 3:b17ae88bfa54 140 _controllerOutput = 0;
Jtroper 3:b17ae88bfa54 141 _errorArea = 0;
Jtroper 3:b17ae88bfa54 142 _BreakSignal = 1;
Jtroper 3:b17ae88bfa54 143 }
Jtroper 3:b17ae88bfa54 144
Jtroper 3:b17ae88bfa54 145 void WaitState()
Jtroper 3:b17ae88bfa54 146 {
Jtroper 3:b17ae88bfa54 147 _State = WAIT;
Jtroper 3:b17ae88bfa54 148 }
Jtroper 3:b17ae88bfa54 149 void RunState()
Jtroper 3:b17ae88bfa54 150 {
Jtroper 3:b17ae88bfa54 151 _State = RUN;
Jtroper 3:b17ae88bfa54 152 }
Jtroper 3:b17ae88bfa54 153 void StopState()
Jtroper 3:b17ae88bfa54 154 {
Jtroper 3:b17ae88bfa54 155 _State = STOP;
Jtroper 2:a17f7da1ca7c 156 }