Interrupt.h

00001 void setup_Interrupt();
00002 void timer1_interrupt();
00003 void LeftWheelInterrupt();
00004 void RightWheelInterrupt();
00005 void GetWheelsSpeed();
00006 void ReachDesiredSpeed();
00007 void UpdateEncoderCoordinate();
00008 
00009 //////////////////////////////////////
00010 Ticker timer1;
00011 
00012 InterruptIn   LeftWheel_PhaseA(A1);
00013 InterruptIn   LeftWheel_PhaseB(A2);
00014 InterruptIn   RightWheel_PhaseA(D13);
00015 InterruptIn   RightWheel_PhaseB(D12);
00016 
00017 bool LeftWheel_PhaseA_State, LeftWheel_PhaseB_State, RightWheel_PhaseA_State, RightWheel_PhaseB_State;
00018 int  LeftWheelState, RightWheelState, Last_Left_Wheel_State, Last_Right_Wheel_State;
00019 ///////
00020 long Timer1Counter;
00021 long long LeftWheelCounter, RightWheelCounter, LastCycleLeftWheelCounter, LastCycleRightWheelCounter;
00022 float LeftWheelRPM, RightWheelRPM;
00023 double LeftWheelSpeed, RightWheelSpeed, DesiredLeftWheelSpeed, DesiredRightWheelSpeed;
00024 float DesiredLeftWheelRPM, DesiredRightWheelRPM;
00025 float LeftLastSpeedErr, RightLastSpeedErr;
00026 float LeftSpeedPgain = 0.001;
00027 float LeftSpeedDgain = 0.000;
00028 float RightSpeedPgain = LeftSpeedPgain;
00029 float RightSpeedDgain = LeftSpeedDgain;
00030 
00031 void Stra1_Rtloop();/////////////////need to be revised
00032 /////////////////////////////////////////////////////////////
00033 void timer1_interrupt()
00034 {
00035     Timer1Counter++;    
00036     GetWheelsSpeed();
00037     Stra1_Rtloop();
00038     
00039     //TransToDesiredWheelsSpeed(double DesiredLinearSpeed, double DesiredAngleSpeed);
00040     ReachDesiredSpeed();
00041     UpdateEncoderCoordinate();
00042     ////
00043     
00044 }
00045 
00046 //////////////////////////////////
00047 
00048 
00049 
00050 void setup_Interrupt()
00051 {
00052     timer1.attach_us(&timer1_interrupt, TIMER_CYCLING_TIME);
00053     //
00054     LeftWheel_PhaseA.rise(&LeftWheelInterrupt);
00055     LeftWheel_PhaseA.fall(&LeftWheelInterrupt);
00056     LeftWheel_PhaseB.rise(&LeftWheelInterrupt);
00057     LeftWheel_PhaseB.fall(&LeftWheelInterrupt);
00058 
00059     RightWheel_PhaseA.rise(&RightWheelInterrupt);
00060     RightWheel_PhaseA.fall(&RightWheelInterrupt);
00061     RightWheel_PhaseB.rise(&RightWheelInterrupt);
00062     RightWheel_PhaseB.fall(&RightWheelInterrupt);
00063 }
00064 
00065 void LeftWheelInterrupt()
00066 {
00067     LeftWheel_PhaseA_State = LeftWheel_PhaseA.read();
00068     LeftWheel_PhaseB_State = LeftWheel_PhaseB.read();
00069     LeftWheelState = 2*LeftWheel_PhaseA_State + LeftWheel_PhaseB_State;
00070     switch(LeftWheelState)
00071     {
00072         case 1:
00073             if(Last_Left_Wheel_State == 0) 
00074             {
00075                 LeftWheelCounter--;                 
00076             } 
00077             else if(Last_Left_Wheel_State == 3) 
00078             {
00079                 LeftWheelCounter++;                 
00080             }
00081             break; 
00082         case 0:
00083             if(Last_Left_Wheel_State == 2) 
00084             {
00085                 LeftWheelCounter--;                 
00086             } 
00087             else if(Last_Left_Wheel_State == 1) 
00088             {
00089                 LeftWheelCounter++;                 
00090             }
00091             break; 
00092         case 2:
00093             if(Last_Left_Wheel_State== 3) 
00094             {
00095                 LeftWheelCounter--;                 
00096             } 
00097             else if(Last_Left_Wheel_State == 0) 
00098             {
00099                 LeftWheelCounter++;                
00100             }
00101             break; 
00102         case 3:
00103             if(Last_Left_Wheel_State == 1) 
00104             {
00105                 LeftWheelCounter--;                 
00106             } 
00107             else if(Last_Left_Wheel_State == 2) 
00108             {
00109                 LeftWheelCounter++;                 
00110             }
00111             break; 
00112     }
00113     Last_Left_Wheel_State = LeftWheelState;
00114 }
00115 void RightWheelInterrupt()
00116 {
00117     RightWheel_PhaseA_State = RightWheel_PhaseA.read();
00118     RightWheel_PhaseB_State = RightWheel_PhaseB.read();
00119     RightWheelState = 2*RightWheel_PhaseA_State + RightWheel_PhaseB_State;
00120     switch(RightWheelState)
00121     {
00122         case 1:
00123             if(Last_Right_Wheel_State == 0) 
00124             {
00125                 RightWheelCounter++;                 
00126             } 
00127             else if(Last_Right_Wheel_State == 3) 
00128             {
00129                 RightWheelCounter--;                 
00130             }
00131             break; 
00132         case 0:
00133             if(Last_Right_Wheel_State == 2) 
00134             {
00135                 RightWheelCounter++;                 
00136             } 
00137             else if(Last_Right_Wheel_State == 1) 
00138             {
00139                 RightWheelCounter--;                 
00140             }
00141             break; 
00142         case 2:
00143             if(Last_Right_Wheel_State== 3) 
00144             {
00145                 RightWheelCounter++;                 
00146             } 
00147             else if(Last_Right_Wheel_State == 0) 
00148             {
00149                 RightWheelCounter--;                
00150             }
00151             break; 
00152         case 3:
00153             if(Last_Right_Wheel_State == 1) 
00154             {
00155                 RightWheelCounter++;                 
00156             } 
00157             else if(Last_Right_Wheel_State == 2) 
00158             {
00159                 RightWheelCounter--;                 
00160             }
00161             break; 
00162     }
00163     Last_Right_Wheel_State = RightWheelState;
00164 }
00165 void TransToDesiredWheelsSpeed(double DesiredLinearSpeed, double DesiredAngleSpeed)  //pixel/s , rad/s
00166 {
00167     DesiredLeftWheelSpeed = DesiredLinearSpeed * 20;// / ENCODER_TO_DISTANCE;
00168     DesiredRightWheelSpeed = DesiredLeftWheelSpeed;
00169     double WheelSpeedDiff = DesiredAngleSpeed * ENCODER_TO_RAD;
00170     DesiredLeftWheelSpeed += WheelSpeedDiff;
00171     DesiredRightWheelSpeed -= WheelSpeedDiff;
00172 }
00173 void GetWheelsSpeed()
00174 {
00175     //LeftWheelRPM = (LeftWheelCounter - LastCycleLeftWheelCounter) * INVERSE_TIMER_CYCLING_TIME * 60 *INVERSE_OneRotationCounts;
00176     //RightWheelRPM = (RightWheelCounter - LastCycleRightWheelCounter) * INVERSE_TIMER_CYCLING_TIME * 60 *INVERSE_OneRotationCounts;
00177     
00178     LeftWheelSpeed = (LeftWheelCounter - LastCycleLeftWheelCounter) * INVERSE_TIMER_CYCLING_TIME; //count/s
00179     RightWheelSpeed = (RightWheelCounter - LastCycleRightWheelCounter) * INVERSE_TIMER_CYCLING_TIME ; //count/s
00180     
00181     /* Move to UpdateEncoderCoordinate()
00182     LastCycleLeftWheelCounter = LeftWheelCounter;
00183     LastCycleRightWheelCounter = RightWheelCounter;
00184     */
00185 }
00186 void ReachDesiredSpeed()
00187 {
00188     float LSpeedErr = DesiredLeftWheelSpeed - LeftWheelSpeed;//DesiredLeftWheelRPM - LeftWheelRPM;
00189     float RSpeedErr = DesiredRightWheelSpeed - RightWheelSpeed;//DesiredRightWheelRPM - RightWheelRPM;
00190     float DiffLSpeedErr = LSpeedErr - LeftLastSpeedErr;
00191     float DiffRSpeedErr = RSpeedErr - RightLastSpeedErr;    
00192     LeftLastSpeedErr = LSpeedErr;
00193     RightLastSpeedErr = RSpeedErr;
00194     
00195     float Loutput = LeftSpeedPgain * LSpeedErr + LeftSpeedDgain * DiffLSpeedErr;
00196     float Routput = RightSpeedPgain * RSpeedErr + RightSpeedDgain * DiffRSpeedErr;
00197     LeftWheelPower = Loutput;
00198     RightWheelPower = Routput;
00199     if(LeftWheelPower > 0.5) LeftWheelPower = 0.5;
00200     if(LeftWheelPower < -0.5) LeftWheelPower = -0.5;
00201     if(RightWheelPower > 0.5) RightWheelPower = 0.5;
00202     if(RightWheelPower < -0.5) RightWheelPower = -0.5;
00203     
00204     OutputLeftWheel(LeftWheelPower);
00205     OutputRightWheel(RightWheelPower);    
00206 }
00207 
00208 void UpdateEncoderCoordinate()
00209 {
00210     long LeftDisplacement = (LeftWheelCounter - LastCycleLeftWheelCounter);
00211     long RightDisplacement = (RightWheelCounter - LastCycleRightWheelCounter);
00212     NowDirectionInRad += (LeftDisplacement - RightDisplacement);//*ENCODER_TO_RAD;
00213     /*
00214     if(NowDirectionInRad > PI)
00215     {
00216         NowDirectionInRad -= 2*PI;
00217         SelfRotation++;
00218     }
00219     else if(NowDirectionInRad < -1*PI)
00220     {
00221         NowDirectionInRad += 2*PI;
00222         SelfRotation--;
00223     }
00224     */
00225     NowCoordinate[0] += (LeftDisplacement + RightDisplacement)*0.5 *ENCODER_TO_DISTANCE * cos(NowDirectionInRad);
00226     NowCoordinate[1] += (LeftDisplacement + RightDisplacement)*0.5 *ENCODER_TO_DISTANCE * sin(NowDirectionInRad);
00227     //
00228     LastCycleLeftWheelCounter = LeftWheelCounter;
00229     LastCycleRightWheelCounter = RightWheelCounter;
00230     //bluetooth.printf("\n%f", NowDirectionInRad);
00231     //uetooth.printf("\n%d, NowDirectionInRad);
00232     //bluetooth.printf("   %f", NowDirectionInRad);
00233 }
00234 
00235 void TurnErrorEliminateWhileForwarding()
00236 {
00237     double TargetAngleInRad = atan2( TargetCoordinate[1] - NowCoordinate[1], TargetCoordinate[0] - NowCoordinate[0]);
00238     double AngleError = TargetAngleInRad - NowDirectionInRad;    
00239 
00240 }