motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
inits.h@17:034b50f49f46, 2015-10-07 (annotated)
- Committer:
- ewoud
- Date:
- Wed Oct 07 12:19:30 2015 +0000
- Revision:
- 17:034b50f49f46
- Parent:
- 16:e9945e3b4712
- Child:
- 18:4ee32b922251
fixed timing issue by introducing a clock to get the loop time
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ewoud | 10:e807253d4879 | 1 | //****************************************************************************/ |
ewoud | 10:e807253d4879 | 2 | // Defines |
ewoud | 10:e807253d4879 | 3 | //****************************************************************************/ |
ewoud | 16:e9945e3b4712 | 4 | #define RATE 0.05 |
ewoud | 16:e9945e3b4712 | 5 | #define calcRATE 0.5 |
ewoud | 14:102a2b4f5c86 | 6 | #define Kc 1.5 |
ewoud | 14:102a2b4f5c86 | 7 | #define Ti 0.8 |
ewoud | 10:e807253d4879 | 8 | #define Td 0.0 |
ewoud | 10:e807253d4879 | 9 | |
ewoud | 10:e807253d4879 | 10 | //****************************************************************************/ |
ewoud | 10:e807253d4879 | 11 | // Globals |
ewoud | 10:e807253d4879 | 12 | //****************************************************************************/ |
ewoud | 10:e807253d4879 | 13 | Serial pc(USBTX, USBRX); |
ewoud | 14:102a2b4f5c86 | 14 | HIDScope scope(5); // Instantize a 2-channel HIDScope object |
ewoud | 10:e807253d4879 | 15 | Ticker scopeTimer; // Instantize the timer for sending data to the PC |
ewoud | 13:40141b362092 | 16 | InterruptIn startButton(D3); |
ewoud | 13:40141b362092 | 17 | InterruptIn stopButton(D2); |
ewoud | 10:e807253d4879 | 18 | //-------- |
ewoud | 10:e807253d4879 | 19 | // Motors |
ewoud | 10:e807253d4879 | 20 | //-------- |
ewoud | 10:e807253d4879 | 21 | // Left motor. |
ewoud | 10:e807253d4879 | 22 | PwmOut leftMotor(D5); |
ewoud | 10:e807253d4879 | 23 | DigitalOut leftDirection(D4); |
ewoud | 10:e807253d4879 | 24 | QEI leftQei(D12, D13, NC, 624); |
ewoud | 10:e807253d4879 | 25 | PID leftController(Kc, Ti, Td, RATE); |
ewoud | 10:e807253d4879 | 26 | |
ewoud | 10:e807253d4879 | 27 | // Right motor |
ewoud | 10:e807253d4879 | 28 | PwmOut rightMotor(D6); |
ewoud | 10:e807253d4879 | 29 | DigitalOut rightDirection(D7); |
ewoud | 12:d7bb475bb82d | 30 | QEI rightQei(D11, D10, NC, 624); |
ewoud | 10:e807253d4879 | 31 | PID rightController(Kc, Ti, Td, RATE); |
ewoud | 10:e807253d4879 | 32 | |
ewoud | 10:e807253d4879 | 33 | // EMG input |
ewoud | 10:e807253d4879 | 34 | AnalogIn pot1(A0); |
ewoud | 10:e807253d4879 | 35 | AnalogIn pot2(A1); |
ewoud | 10:e807253d4879 | 36 | |
ewoud | 10:e807253d4879 | 37 | |
ewoud | 10:e807253d4879 | 38 | // Timers |
ewoud | 10:e807253d4879 | 39 | Ticker motorControlTicker; |
ewoud | 16:e9945e3b4712 | 40 | Ticker speedcalcTicker; |
ewoud | 17:034b50f49f46 | 41 | Timer velocityTimer; |
ewoud | 17:034b50f49f46 | 42 | float looptime; |
ewoud | 17:034b50f49f46 | 43 | float lasttime=0; |
ewoud | 17:034b50f49f46 | 44 | float thistime; |
ewoud | 10:e807253d4879 | 45 | bool goFlag=false; |
ewoud | 16:e9945e3b4712 | 46 | bool calcFlag=false; |
ewoud | 17:034b50f49f46 | 47 | bool didCalc=false; |
ewoud | 13:40141b362092 | 48 | bool systemOn=false; |
ewoud | 14:102a2b4f5c86 | 49 | |
ewoud | 14:102a2b4f5c86 | 50 | // Working variables: motors |
ewoud | 10:e807253d4879 | 51 | volatile int leftPulses = 0; |
ewoud | 10:e807253d4879 | 52 | volatile int leftPrevPulses = 0; |
ewoud | 10:e807253d4879 | 53 | volatile float leftPwmDuty = 0.0; |
ewoud | 10:e807253d4879 | 54 | volatile float leftPwmDutyPrev = 0.0; |
ewoud | 10:e807253d4879 | 55 | volatile float leftVelocity = 0.0; |
ewoud | 10:e807253d4879 | 56 | |
ewoud | 10:e807253d4879 | 57 | volatile int rightPulses = 0; |
ewoud | 10:e807253d4879 | 58 | volatile int rightPrevPulses = 0; |
ewoud | 10:e807253d4879 | 59 | volatile float rightPwmDuty = 0.0; |
ewoud | 10:e807253d4879 | 60 | volatile float rightPwmDutyPrev = 0.0; |
ewoud | 10:e807253d4879 | 61 | volatile float rightVelocity = 0.0; |
ewoud | 10:e807253d4879 | 62 | |
ewoud | 14:102a2b4f5c86 | 63 | // request calculation variables |
ewoud | 10:e807253d4879 | 64 | float request; |
ewoud | 10:e807253d4879 | 65 | float request_pos; |
ewoud | 10:e807253d4879 | 66 | float request_neg; |
ewoud | 14:102a2b4f5c86 | 67 | float leftAngle; |
ewoud | 14:102a2b4f5c86 | 68 | float rightAngle; |
ewoud | 16:e9945e3b4712 | 69 | //float round=4200; |
ewoud | 16:e9945e3b4712 | 70 | float round=28000; // including extra gear. |
ewoud | 14:102a2b4f5c86 | 71 | float toX; |
ewoud | 14:102a2b4f5c86 | 72 | float toY; |
ewoud | 14:102a2b4f5c86 | 73 | float leftDeltaAngle; |
ewoud | 14:102a2b4f5c86 | 74 | float rightDeltaAngle; |
ewoud | 14:102a2b4f5c86 | 75 | float leftRequest; |
ewoud | 14:102a2b4f5c86 | 76 | float rightRequest; |
ewoud | 14:102a2b4f5c86 | 77 | float currentX; |
ewoud | 14:102a2b4f5c86 | 78 | float currentY; |
ewoud | 14:102a2b4f5c86 | 79 | float toLeftAngle; |
ewoud | 14:102a2b4f5c86 | 80 | float toRightAngle; |
ewoud | 14:102a2b4f5c86 | 81 | const double M_PI =3.141592653589793238463; |
ewoud | 16:e9945e3b4712 | 82 | const float l = 100; // distance between the motors |
ewoud | 16:e9945e3b4712 | 83 | const float armlength=150; // length of the arms from the motor |
ewoud | 10:e807253d4879 | 84 | |
ewoud | 10:e807253d4879 | 85 | void initMotors(){ |
ewoud | 10:e807253d4879 | 86 | //Initialization of motor |
ewoud | 10:e807253d4879 | 87 | leftMotor.period_us(50); |
ewoud | 10:e807253d4879 | 88 | leftMotor = 0.0; |
ewoud | 10:e807253d4879 | 89 | leftDirection = 1; |
ewoud | 10:e807253d4879 | 90 | |
ewoud | 10:e807253d4879 | 91 | rightMotor.period_us(50); |
ewoud | 10:e807253d4879 | 92 | rightMotor = 0.0; |
ewoud | 14:102a2b4f5c86 | 93 | rightDirection = 0; |
ewoud | 10:e807253d4879 | 94 | |
ewoud | 10:e807253d4879 | 95 | } |
ewoud | 10:e807253d4879 | 96 | |
ewoud | 10:e807253d4879 | 97 | void initPIDs(){ |
ewoud | 10:e807253d4879 | 98 | //Initialization of PID controller |
ewoud | 10:e807253d4879 | 99 | leftController.setInputLimits(-1.0, 1.0); |
ewoud | 10:e807253d4879 | 100 | leftController.setOutputLimits(-1.0, 1.0); |
ewoud | 10:e807253d4879 | 101 | leftController.setBias(0); |
ewoud | 10:e807253d4879 | 102 | leftController.setDeadzone(-0.4, 0.4); |
ewoud | 10:e807253d4879 | 103 | leftController.setMode(AUTO_MODE); |
ewoud | 10:e807253d4879 | 104 | |
ewoud | 10:e807253d4879 | 105 | rightController.setInputLimits(-1.0, 1.0); |
ewoud | 10:e807253d4879 | 106 | rightController.setOutputLimits(-1.0, 1.0); |
ewoud | 10:e807253d4879 | 107 | rightController.setBias(0); |
ewoud | 10:e807253d4879 | 108 | rightController.setDeadzone(-0.4, 0.4); |
ewoud | 10:e807253d4879 | 109 | rightController.setMode(AUTO_MODE); |
ewoud | 10:e807253d4879 | 110 | } |