Group19-Biorobotics
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_control_v3 by
Controller.h@8:82e38f4aa690, 2016-11-07 (annotated)
- Committer:
- AlexGroup19
- Date:
- Mon Nov 07 11:29:28 2016 +0000
- Revision:
- 8:82e38f4aa690
- Parent:
- 6:a4440eec3652
Group19-Biorobotics
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 | 6:a4440eec3652 | 5 | |
fabian101 | 6:a4440eec3652 | 6 | MODSERIAL pc(USBTX, USBRX); |
fabian101 | 6:a4440eec3652 | 7 | //HIDScope scope(2); |
fabian101 | 1:b9005f2aabf5 | 8 | |
fabian101 | 2:6b913f93bc0b | 9 | int xPulses = 0; // pulsecount |
AlexGroup19 | 8:82e38f4aa690 | 10 | const int x_max = 31068;// 30068; |
fabian101 | 2:6b913f93bc0b | 11 | int yPulses = 0; |
fabian101 | 6:a4440eec3652 | 12 | const int y_max = 15003; |
fabian101 | 2:6b913f93bc0b | 13 | int prevXpulses = 0; |
fabian101 | 2:6b913f93bc0b | 14 | int prevYpulses = 0; |
fabian101 | 1:b9005f2aabf5 | 15 | |
fabian101 | 2:6b913f93bc0b | 16 | float motorValueX = 0; // requested speed |
fabian101 | 2:6b913f93bc0b | 17 | float motorValueY = 0; |
fabian101 | 0:d935ea6f5c25 | 18 | |
fabian101 | 6:a4440eec3652 | 19 | const float maxXValue = 1.0; // max motorvalue |
fabian101 | 6:a4440eec3652 | 20 | const float maxYValue = 1.0; |
fabian101 | 1:b9005f2aabf5 | 21 | |
fabian101 | 6:a4440eec3652 | 22 | const float maxSpeed = 1; // cm/s |
fabian101 | 6:a4440eec3652 | 23 | |
fabian101 | 6:a4440eec3652 | 24 | float X_frictionTrheshold = 0.04; |
fabian101 | 6:a4440eec3652 | 25 | float Y_frictionTrheshold = 0.04; |
fabian101 | 6:a4440eec3652 | 26 | float sensitivityFactor = 0.2; // determines treshold value for contraction, value between 0 and 1 |
AlexGroup19 | 8:82e38f4aa690 | 27 | float calibrationSpeed = 0.4; // [0,1], determines how fast the system returns to (0,0) when the button is pressed |
fabian101 | 0:d935ea6f5c25 | 28 | |
fabian101 | 2:6b913f93bc0b | 29 | const float R1 = 0.085; // radius of big driven gear in m |
fabian101 | 2:6b913f93bc0b | 30 | const float RmX = 0.012; // radius of small drive gear in m |
fabian101 | 2:6b913f93bc0b | 31 | const float Rpulley = 0.015915; // radius of pulley gear in m |
fabian101 | 1:b9005f2aabf5 | 32 | |
fabian101 | 2:6b913f93bc0b | 33 | float prevX = 0; |
fabian101 | 1:b9005f2aabf5 | 34 | float prevY = 0; |
fabian101 | 1:b9005f2aabf5 | 35 | |
fabian101 | 6:a4440eec3652 | 36 | float r = 0; |
fabian101 | 6:a4440eec3652 | 37 | float l = 0; |
fabian101 | 6:a4440eec3652 | 38 | float u = 0; |
fabian101 | 6:a4440eec3652 | 39 | float d = 0; |
fabian101 | 6:a4440eec3652 | 40 | float rMax = 0; // maxAmplitude emg signal |
fabian101 | 6:a4440eec3652 | 41 | float lMax = 0; |
fabian101 | 6:a4440eec3652 | 42 | float uMax = 0; |
fabian101 | 6:a4440eec3652 | 43 | float dMax = 0; |
fabian101 | 6:a4440eec3652 | 44 | float percentageR = 0; |
fabian101 | 6:a4440eec3652 | 45 | float percentageL = 0; |
fabian101 | 6:a4440eec3652 | 46 | float percentageU = 0; |
fabian101 | 6:a4440eec3652 | 47 | float percentageD = 0; |
fabian101 | 6:a4440eec3652 | 48 | |
fabian101 | 4:bfdcf3da7cff | 49 | int i = 0; |
fabian101 | 2:6b913f93bc0b | 50 | |
fabian101 | 1:b9005f2aabf5 | 51 | const int margin = 100; // margin pulses for calibration |
fabian101 | 0:d935ea6f5c25 | 52 | |
fabian101 | 0:d935ea6f5c25 | 53 | void calibrate(); |
fabian101 | 1:b9005f2aabf5 | 54 | void calibratePower(); |
fabian101 | 0:d935ea6f5c25 | 55 | void run(); |
fabian101 | 1:b9005f2aabf5 | 56 | void initControllers(); |
fabian101 | 1:b9005f2aabf5 | 57 | void initMotors(); |
fabian101 | 0:d935ea6f5c25 | 58 | |
AlexGroup19 | 8:82e38f4aa690 | 59 | const float KcX = 0.02058; // THIS IS THE LIMIT FOR STABILITY |
AlexGroup19 | 8:82e38f4aa690 | 60 | const float KcY = 0.02066; |
fabian101 | 1:b9005f2aabf5 | 61 | const float Ti = 0.0; |
fabian101 | 1:b9005f2aabf5 | 62 | const float Td = 0.0; |
fabian101 | 6:a4440eec3652 | 63 | const float RATE = 0.002; // rate = interval motors will be updateed |
fabian101 | 4:bfdcf3da7cff | 64 | |
fabian101 | 6:a4440eec3652 | 65 | int counter = 1; |
fabian101 | 2:6b913f93bc0b | 66 | |
fabian101 | 2:6b913f93bc0b | 67 | PwmOut X_Magnitude(D5); // bind to horizontal motor |
fabian101 | 2:6b913f93bc0b | 68 | DigitalOut X_Direction(D4); |
fabian101 | 6:a4440eec3652 | 69 | PID X_Controller(KcX, Ti, Td, RATE); |
fabian101 | 6:a4440eec3652 | 70 | QEI X_Motor(D13 ,D12 ,NC, 64 ,QEI::X4_ENCODING); |
fabian101 | 1:b9005f2aabf5 | 71 | |
fabian101 | 6:a4440eec3652 | 72 | PwmOut Y_Magnitude(D6); // bind to vertical motor |
fabian101 | 2:6b913f93bc0b | 73 | DigitalOut Y_Direction(D7); |
fabian101 | 6:a4440eec3652 | 74 | PID Y_Controller(KcY, Ti, Td, RATE); |
fabian101 | 6:a4440eec3652 | 75 | QEI Y_Motor(D15 ,D14 ,NC, 64 ,QEI::X4_ENCODING); |
fabian101 | 1:b9005f2aabf5 | 76 | |
fabian101 | 4:bfdcf3da7cff | 77 | //InterruptIn calibrateButton(D2); |
fabian101 | 4:bfdcf3da7cff | 78 | DigitalIn button(D2); |
fabian101 | 0:d935ea6f5c25 | 79 | Ticker motorControl; |
fabian101 | 4:bfdcf3da7cff | 80 | void setUpdate_flag(); |
fabian101 | 4:bfdcf3da7cff | 81 | volatile bool update_flag = false; |
fabian101 | 6:a4440eec3652 | 82 | const int CW = false; |
fabian101 | 2:6b913f93bc0b | 83 | |
fabian101 | 4:bfdcf3da7cff | 84 | DigitalOut led(LED_BLUE); |
fabian101 | 1:b9005f2aabf5 | 85 | |
fabian101 | 1:b9005f2aabf5 | 86 | void initMotors(){ |
fabian101 | 4:bfdcf3da7cff | 87 | X_Magnitude.period_ms(1); |
fabian101 | 2:6b913f93bc0b | 88 | X_Magnitude.write(0.0); |
fabian101 | 2:6b913f93bc0b | 89 | X_Direction.write(CW); |
fabian101 | 1:b9005f2aabf5 | 90 | |
fabian101 | 4:bfdcf3da7cff | 91 | Y_Magnitude.period_ms(1); |
fabian101 | 2:6b913f93bc0b | 92 | Y_Magnitude.write(0.0); |
fabian101 | 2:6b913f93bc0b | 93 | Y_Direction.write(CW); |
fabian101 | 6:a4440eec3652 | 94 | led = true; |
fabian101 | 1:b9005f2aabf5 | 95 | } |
fabian101 | 1:b9005f2aabf5 | 96 | void initControllers(){ |
fabian101 | 6:a4440eec3652 | 97 | X_Controller.setInputLimits(-maxSpeed,maxSpeed); // max output will be 10% |
fabian101 | 2:6b913f93bc0b | 98 | X_Controller.setOutputLimits(-1.0,1.0); |
fabian101 | 2:6b913f93bc0b | 99 | X_Controller.setBias(0); |
fabian101 | 6:a4440eec3652 | 100 | X_Controller.setMode(1); |
fabian101 | 1:b9005f2aabf5 | 101 | |
fabian101 | 6:a4440eec3652 | 102 | Y_Controller.setInputLimits(-maxSpeed,maxSpeed); // fix me |
fabian101 | 2:6b913f93bc0b | 103 | Y_Controller.setOutputLimits(-1.0,1.0); |
fabian101 | 2:6b913f93bc0b | 104 | Y_Controller.setBias(0); |
fabian101 | 6:a4440eec3652 | 105 | Y_Controller.setMode(1); |
fabian101 | 1:b9005f2aabf5 | 106 | } |