Group19-Biorobotics
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_control_v3 by
Controller.h@4:bfdcf3da7cff, 2016-10-28 (annotated)
- Committer:
- fabian101
- Date:
- Fri Oct 28 12:49:02 2016 +0000
- Revision:
- 4:bfdcf3da7cff
- Parent:
- 2:6b913f93bc0b
- Child:
- 6:a4440eec3652
28-10, filtering needs tweaking, controller needs testing, some fix-me's need to be done
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fabian101 | 4:bfdcf3da7cff | 1 | #include "SigInterpreter.h" |
fabian101 | 4:bfdcf3da7cff | 2 | |
fabian101 | 4:bfdcf3da7cff | 3 | SigInterpreter signal = SigInterpreter(); |
fabian101 | 1:b9005f2aabf5 | 4 | const double PI = 3.141592653589793; |
fabian101 | 4:bfdcf3da7cff | 5 | //MODSERIAL pc(USBTX, USBRX); |
fabian101 | 1:b9005f2aabf5 | 6 | |
fabian101 | 2:6b913f93bc0b | 7 | int xPulses = 0; // pulsecount |
fabian101 | 4:bfdcf3da7cff | 8 | const int x_max = 33068; |
fabian101 | 2:6b913f93bc0b | 9 | int yPulses = 0; |
fabian101 | 4:bfdcf3da7cff | 10 | const int y_max = 17640; |
fabian101 | 2:6b913f93bc0b | 11 | int prevXpulses = 0; |
fabian101 | 2:6b913f93bc0b | 12 | int prevYpulses = 0; |
fabian101 | 1:b9005f2aabf5 | 13 | |
fabian101 | 4:bfdcf3da7cff | 14 | double rMax = 0; // maxAmplitude emg signal |
fabian101 | 4:bfdcf3da7cff | 15 | double lMax = 0; |
fabian101 | 4:bfdcf3da7cff | 16 | double uMax = 0; |
fabian101 | 4:bfdcf3da7cff | 17 | double dMax = 0; |
fabian101 | 1:b9005f2aabf5 | 18 | |
fabian101 | 2:6b913f93bc0b | 19 | float motorValueX = 0; // requested speed |
fabian101 | 2:6b913f93bc0b | 20 | float motorValueY = 0; |
fabian101 | 0:d935ea6f5c25 | 21 | |
fabian101 | 2:6b913f93bc0b | 22 | const float maxXValue = 1.0f; // v1 in drawing, max speed cm/s |
fabian101 | 2:6b913f93bc0b | 23 | const float maxYValue = 1.0f; // |
fabian101 | 1:b9005f2aabf5 | 24 | |
fabian101 | 2:6b913f93bc0b | 25 | const float maxSpeed = 0.01f; // m/s |
fabian101 | 0:d935ea6f5c25 | 26 | |
fabian101 | 2:6b913f93bc0b | 27 | const float R1 = 0.085; // radius of big driven gear in m |
fabian101 | 2:6b913f93bc0b | 28 | const float RmX = 0.012; // radius of small drive gear in m |
fabian101 | 2:6b913f93bc0b | 29 | const float Rpulley = 0.015915; // radius of pulley gear in m |
fabian101 | 1:b9005f2aabf5 | 30 | |
fabian101 | 2:6b913f93bc0b | 31 | float prevX = 0; |
fabian101 | 1:b9005f2aabf5 | 32 | float prevY = 0; |
fabian101 | 1:b9005f2aabf5 | 33 | |
fabian101 | 2:6b913f93bc0b | 34 | double r = 0; |
fabian101 | 2:6b913f93bc0b | 35 | double l = 0; |
fabian101 | 2:6b913f93bc0b | 36 | double u = 0; |
fabian101 | 2:6b913f93bc0b | 37 | double d = 0; |
fabian101 | 4:bfdcf3da7cff | 38 | int i = 0; |
fabian101 | 2:6b913f93bc0b | 39 | |
fabian101 | 4:bfdcf3da7cff | 40 | const int SAMPLES_PER_AVERAGE = 10; |
fabian101 | 1:b9005f2aabf5 | 41 | const int margin = 100; // margin pulses for calibration |
fabian101 | 0:d935ea6f5c25 | 42 | |
fabian101 | 0:d935ea6f5c25 | 43 | void calibrate(); |
fabian101 | 1:b9005f2aabf5 | 44 | void calibratePower(); |
fabian101 | 0:d935ea6f5c25 | 45 | void run(); |
fabian101 | 0:d935ea6f5c25 | 46 | double toMotorValue(double,double); |
fabian101 | 0:d935ea6f5c25 | 47 | void emergencyExit(); |
fabian101 | 0:d935ea6f5c25 | 48 | void motorOutput(double, bool); |
fabian101 | 1:b9005f2aabf5 | 49 | void initControllers(); |
fabian101 | 1:b9005f2aabf5 | 50 | void initMotors(); |
fabian101 | 2:6b913f93bc0b | 51 | void initMotorvariables(); |
fabian101 | 0:d935ea6f5c25 | 52 | |
fabian101 | 4:bfdcf3da7cff | 53 | HIDScope scope(2); |
fabian101 | 0:d935ea6f5c25 | 54 | |
fabian101 | 1:b9005f2aabf5 | 55 | const float Kc = 0.0; |
fabian101 | 1:b9005f2aabf5 | 56 | const float Ti = 0.0; |
fabian101 | 1:b9005f2aabf5 | 57 | const float Td = 0.0; |
fabian101 | 4:bfdcf3da7cff | 58 | const float RATE = 0.001; // rate = interval motors will be updateed |
fabian101 | 4:bfdcf3da7cff | 59 | |
fabian101 | 4:bfdcf3da7cff | 60 | int counter = 0; |
fabian101 | 4:bfdcf3da7cff | 61 | int counter_motor = 0; |
fabian101 | 2:6b913f93bc0b | 62 | |
fabian101 | 2:6b913f93bc0b | 63 | PwmOut X_Magnitude(D5); // bind to horizontal motor |
fabian101 | 2:6b913f93bc0b | 64 | DigitalOut X_Direction(D4); |
fabian101 | 2:6b913f93bc0b | 65 | PID X_Controller(Kc, Ti, Td, RATE); |
fabian101 | 4:bfdcf3da7cff | 66 | QEI X_Motor(D12 ,D13 ,NC, 64 ,QEI::X4_ENCODING); |
fabian101 | 1:b9005f2aabf5 | 67 | |
fabian101 | 2:6b913f93bc0b | 68 | PwmOut Y_Magnitude(D6); // bind to Ytical motor |
fabian101 | 2:6b913f93bc0b | 69 | DigitalOut Y_Direction(D7); |
fabian101 | 2:6b913f93bc0b | 70 | PID Y_Controller(Kc, Ti, Td, RATE); |
fabian101 | 4:bfdcf3da7cff | 71 | QEI Y_Motor(D14 ,D15 ,NC, 64 ,QEI::X4_ENCODING); |
fabian101 | 1:b9005f2aabf5 | 72 | |
fabian101 | 4:bfdcf3da7cff | 73 | //InterruptIn calibrateButton(D2); |
fabian101 | 4:bfdcf3da7cff | 74 | DigitalIn button(D2); |
fabian101 | 0:d935ea6f5c25 | 75 | Ticker motorControl; |
fabian101 | 4:bfdcf3da7cff | 76 | void setUpdate_flag(); |
fabian101 | 4:bfdcf3da7cff | 77 | volatile bool update_flag = false; |
fabian101 | 4:bfdcf3da7cff | 78 | const int CW = true; |
fabian101 | 2:6b913f93bc0b | 79 | |
fabian101 | 4:bfdcf3da7cff | 80 | DigitalOut led(LED_BLUE); |
fabian101 | 1:b9005f2aabf5 | 81 | |
fabian101 | 1:b9005f2aabf5 | 82 | void initMotors(){ |
fabian101 | 4:bfdcf3da7cff | 83 | X_Magnitude.period_ms(1); |
fabian101 | 2:6b913f93bc0b | 84 | X_Magnitude.write(0.0); |
fabian101 | 2:6b913f93bc0b | 85 | X_Direction.write(CW); |
fabian101 | 1:b9005f2aabf5 | 86 | |
fabian101 | 4:bfdcf3da7cff | 87 | Y_Magnitude.period_ms(1); |
fabian101 | 2:6b913f93bc0b | 88 | Y_Magnitude.write(0.0); |
fabian101 | 2:6b913f93bc0b | 89 | Y_Direction.write(CW); |
fabian101 | 1:b9005f2aabf5 | 90 | } |
fabian101 | 1:b9005f2aabf5 | 91 | void initControllers(){ |
fabian101 | 4:bfdcf3da7cff | 92 | X_Controller.setInputLimits(-1.0,1.0); // fiX_ me |
fabian101 | 2:6b913f93bc0b | 93 | X_Controller.setOutputLimits(-1.0,1.0); |
fabian101 | 2:6b913f93bc0b | 94 | X_Controller.setBias(0); |
fabian101 | 4:bfdcf3da7cff | 95 | X_Controller.setMode(0); // fix me |
fabian101 | 1:b9005f2aabf5 | 96 | |
fabian101 | 4:bfdcf3da7cff | 97 | Y_Controller.setInputLimits(-1.0,1.0); // fix me |
fabian101 | 2:6b913f93bc0b | 98 | Y_Controller.setOutputLimits(-1.0,1.0); |
fabian101 | 2:6b913f93bc0b | 99 | Y_Controller.setBias(0); |
fabian101 | 2:6b913f93bc0b | 100 | Y_Controller.setMode(0); |
fabian101 | 1:b9005f2aabf5 | 101 | } |