OperationMode.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "MODSERIAL.h"
00004 #include "QEI.h"
00005 #include "BiQuad.h"
00006 
00007 //Tickers
00008 Ticker TickerMeasureAndControl;
00009 Timer   TimerLoop;
00010 //Communication
00011 MODSERIAL pc(USBTX,USBRX);
00012 QEI Encoder(D10,D11,NC,32);
00013 QEI Encoder2(D12,D13,NC,32);
00014 //Global pin variables
00015 PwmOut PwmPin(D5);
00016 PwmOut PwmPin2(D6);
00017 DigitalOut DirectionPin(D4);
00018 DigitalOut DirectionPin2(D7);
00019 DigitalIn BUT1(D8);
00020 DigitalIn BUT2(D9);
00021 DigitalIn button_sw2(SW2);
00022 DigitalIn button_sw3(SW3);
00023 
00024 DigitalOut LedGreen(LED_GREEN);
00025 DigitalOut LedRed(LED_RED);
00026 DigitalOut LedBlue(LED_BLUE);
00027 
00028 double q1Ref=0;
00029 double q2Ref =0;
00030 
00031 
00032 
00033 
00034 //Define substates Operation Mode
00035 
00036 enum States {OPWait, OPSet, OPState1, OPState2, OPState3, OPState4, OPHoming};
00037 States CurrentOperationState;
00038 
00039 void SetMotorsOff()
00040 {
00041     PwmPin = 0;
00042     PwmPin2 = 0;
00043 }
00044 //State Machine for OperationMode
00045 void OperationStateMachine()
00046 {
00047     switch (CurrentOperationState) {
00048         case OPWait:
00049             if (BUT1 == false) {    //When Left Biceps are contracted, initiate chain to flip the page
00050                 CurrentOperationState = OPState1;
00051                 TimerLoop.reset();
00052             }
00053             if (BUT2 == false) {  //When sw3 is pressed, go to homing for Operation Mode
00054                 CurrentOperationState = OPSet;
00055                 TimerLoop.reset();
00056             }
00057             break;
00058 
00059         case OPSet:                 // Set new homing for Operation Mode and go back to waiting mode
00060             if (q2Ref  > -0.733*(6.380)) {
00061                 q2Ref  -= 0.0005*(6.380);
00062             }
00063             if (q2Ref  < -0.733*(6.380) && TimerLoop > 2) {
00064                 CurrentOperationState = OPWait;
00065                 TimerLoop.reset();
00066             }
00067             break;
00068 
00069         case OPState1:                      // First step of chain to flip the page
00070             if (q1Ref < 0.48*(6.380)) { //counts/8400 *2 because encoder X2 -> X4
00071                 q1Ref += 0.0005*(6.380);
00072             }
00073             if ((q1Ref > 0.48*(6.380)) && (TimerLoop >= 2.0)) {
00074                 CurrentOperationState = OPState2;
00075                 TimerLoop.reset();
00076             }
00077             break;
00078 
00079         case OPState2:                      //Second step of chain to flip the page
00080             if (q2Ref  > -1.133*(6.380)) {
00081                 q2Ref  -= 0.0005*(6.380);
00082             }
00083             if ((q2Ref  < -1.133*(6.380)) && (TimerLoop >= 2.0)) {
00084                 CurrentOperationState = OPState3;
00085                 TimerLoop.reset();
00086             }
00087             break;
00088 
00089         case OPState3:                  //Third step of chain to flip the page
00090             if (q1Ref > -0.15*(6.380)) {
00091                 q1Ref -= 0.0005*(6.380);
00092             }
00093             if (q2Ref  > -1.483*(6.380)) {
00094                 q2Ref  -= 0.0003*(6.380);
00095             }
00096             if ((q1Ref < -0.15*(6.380)) && (q2Ref  < -1.483*(6.380)) && (TimerLoop >= 3.0)) {
00097                 CurrentOperationState = OPState4;
00098                 TimerLoop.reset();
00099             }
00100             break;
00101 
00102         case OPState4:                          //Fourth step of chain to flip the page
00103             if (q2Ref  > -2.133*(6.380)) {
00104                 q2Ref  -= 0.005*(6.380);
00105             }
00106             if ((q2Ref  < -2.133*(6.380)) && (TimerLoop > 0.5)) {
00107                 TimerLoop.reset();
00108                 CurrentOperationState = OPHoming;
00109             }
00110             break;
00111 
00112         case OPHoming:                          //Go back to Home for Operation Mode and back to Waiting
00113             if (q1Ref < 0) {
00114                 q1Ref += 0.003*(6.380);
00115             }
00116             if (q2Ref  < -0.733*(6.380)) {
00117                 q2Ref  += 0.001*(6.380);
00118             }
00119             if ((q1Ref > 0) && (q2Ref  > -0.733*(6.380)) && (TimerLoop > 3)) {
00120                 CurrentOperationState = OPWait;
00121             }
00122             break;
00123     }
00124 }
00125 //Homing 
00126 /*
00127 if (q1Ref >= 0) {
00128                 q1Ref -= 0.001*(6.380);
00129             }
00130             if (q1Ref <= 0) {
00131                 q1Ref += 0.001*(6.380);
00132             }
00133             if (q2Ref >= 0) {
00134                 q2Ref -= 0.001*(6.380);
00135             }
00136             if (q2Ref <= 0) {
00137                 q2Ref += 0.001*(6.380);
00138             }
00139             if ((-0.01*(6.380) < q1Ref < 0.01*(6.380)) && (-0.01*(6.380) < q1Ref < 0.01*(6.380))) {
00140                 SetMotorsOff();
00141             }
00142 */
00143 
00144 //-----------------------------------------------------------------------------
00145 //The double-functions
00146 
00147 
00148 
00149 
00150 // actual position of the motor 1
00151 double GetActualPosition()
00152 {
00153     //This function determines the actual position of the motor
00154     //The count:radians relation is 8400:2pi
00155     double EncoderCounts = Encoder.getPulses();    //number of counts
00156     double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding
00157 
00158     return PositionMotor;
00159 }
00160 
00161 double GetActualPosition2() //motor 2
00162 {
00163     double Encoder2Counts = Encoder2.getPulses();    //number of counts
00164     double PositionMotor2 = Encoder2Counts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding
00165 
00166     return PositionMotor2;
00167 }
00168 
00169 ///The controller
00170 double PID_Controller(double Error)
00171 {
00172     //Arm drive parameters
00173     double Ts = 0.002; //Sampling time 100 Hz
00174     double Kp = 19.8; // proportional gain
00175     double Ki = 3.98; //Intergral gain
00176     double Kd = 1.96; //Differential gain.
00177 
00178     static double ErrorIntegral = 0;
00179     static double ErrorPrevious = Error;
00180     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
00181 
00182     //Proportional part:
00183     double u_k = Kp * Error;
00184 
00185     //Integral part:
00186     ErrorIntegral = ErrorIntegral + Error*Ts;
00187     double u_i = Ki * ErrorIntegral;
00188 
00189     //Derivative part:
00190     double ErrorDerivative = (Error - ErrorPrevious)/Ts;
00191     double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
00192     double u_d = Kd * FilteredErrorDerivative;
00193     ErrorPrevious = Error;
00194 
00195     // sum of parts and return it
00196     return u_k + u_i + u_d; //This will become the MotorValue
00197 }
00198 
00199 double PID_Controller2(double Error)
00200 {
00201     //Belt drive parameters
00202     double Ts = 0.002; //Sampling time 100 Hz
00203     double Kp = 11.1; // proportional gain
00204     double Ki = 2.24; //Integral gain
00205     double Kd = 1.1; //Differential gain
00206 
00207     static double ErrorIntegral = 0;
00208     static double ErrorPrevious = Error;
00209     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
00210 
00211     //Proportional part:
00212     double u_k = Kp * Error;
00213 
00214     //Integral part:
00215     ErrorIntegral = ErrorIntegral + Error*Ts;
00216     double u_i = Ki * ErrorIntegral;
00217 
00218     //Derivative part:
00219     double ErrorDerivative = (Error - ErrorPrevious)/Ts;
00220     double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
00221     double u_d = Kd * FilteredErrorDerivative;
00222     ErrorPrevious = Error;
00223 
00224     // sum of parts and return it
00225     return u_k + u_i + u_d; //This will become the MotorValue
00226 }
00227 
00228 //Ticker function set motorvalues
00229 void SetMotor(double MotorValue, double MotorValue2)
00230 {
00231     if (MotorValue >=0) {
00232         DirectionPin = 1;
00233     } else {
00234         DirectionPin = 0;
00235     }
00236 
00237     if (fabs(MotorValue)>1) { // if error more than 1 radian, full duty cycle
00238         PwmPin = 1;
00239     } else {
00240         PwmPin = fabs(MotorValue);
00241     }
00242 
00243     if (MotorValue2 >=0) {
00244         DirectionPin2 = 1;
00245     } else {
00246         DirectionPin2 = 0;
00247     }
00248 
00249     if (fabs(MotorValue2)>1) { // if error more than 1 radian, full duty cycle
00250         PwmPin2 = 1;
00251     } else {
00252         PwmPin2 = fabs(MotorValue2);
00253     }
00254 }
00255 
00256 // ----------------------------------------------------------------------------
00257 //Ticker function
00258 void MeasureAndControl(void)
00259 {
00260 
00261     //double q1Ref = GetReferencePosition();
00262     //double q2Ref  = GetReferencePosition2();
00263 
00264     OperationStateMachine();
00265     double PositionMotor = GetActualPosition();
00266     double PositionMotor2 = GetActualPosition2();
00267     double MotorValue = PID_Controller(q1Ref - PositionMotor); // input is error
00268     double MotorValue2 = PID_Controller2(q2Ref  - PositionMotor2);
00269     SetMotor(MotorValue, MotorValue2);
00270 }
00271 
00272 
00273 
00274 //-----------------------------------------------------------------------------
00275 int main()
00276 {
00277     LedGreen = 1;
00278     LedRed = 1;
00279     LedBlue = 1;
00280     pc.baud(115200);
00281     pc.printf("Hello World\n\r");
00282     TimerLoop.start();
00283     CurrentOperationState = OPWait;
00284     PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!)
00285     TickerMeasureAndControl.attach(&MeasureAndControl,0.002); //500 Hz
00286     while (true) {
00287     }
00288 }