![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
biorobotics group 19 , 2 november
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_controlv2 by
Controller.h@7:2ab4bdd1f998, 2016-11-02 (annotated)
- Committer:
- fabian101
- Date:
- Wed Nov 02 15:40:15 2016 +0000
- Revision:
- 7:2ab4bdd1f998
- Parent:
- 6:a4440eec3652
biorobotics group 19 , 2 november
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 |
fabian101 | 6:a4440eec3652 | 10 | const int x_max = 30068;// 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 |
fabian101 | 0:d935ea6f5c25 | 27 | |
fabian101 | 2:6b913f93bc0b | 28 | const float R1 = 0.085; // radius of big driven gear in m |
fabian101 | 2:6b913f93bc0b | 29 | const float RmX = 0.012; // radius of small drive gear in m |
fabian101 | 2:6b913f93bc0b | 30 | const float Rpulley = 0.015915; // radius of pulley gear in m |
fabian101 | 1:b9005f2aabf5 | 31 | |
fabian101 | 2:6b913f93bc0b | 32 | float prevX = 0; |
fabian101 | 1:b9005f2aabf5 | 33 | float prevY = 0; |
fabian101 | 1:b9005f2aabf5 | 34 | |
fabian101 | 6:a4440eec3652 | 35 | float r = 0; |
fabian101 | 6:a4440eec3652 | 36 | float l = 0; |
fabian101 | 6:a4440eec3652 | 37 | float u = 0; |
fabian101 | 6:a4440eec3652 | 38 | float d = 0; |
fabian101 | 6:a4440eec3652 | 39 | float rMax = 0; // maxAmplitude emg signal |
fabian101 | 6:a4440eec3652 | 40 | float lMax = 0; |
fabian101 | 6:a4440eec3652 | 41 | float uMax = 0; |
fabian101 | 6:a4440eec3652 | 42 | float dMax = 0; |
fabian101 | 6:a4440eec3652 | 43 | float percentageR = 0; |
fabian101 | 6:a4440eec3652 | 44 | float percentageL = 0; |
fabian101 | 6:a4440eec3652 | 45 | float percentageU = 0; |
fabian101 | 6:a4440eec3652 | 46 | float percentageD = 0; |
fabian101 | 6:a4440eec3652 | 47 | |
fabian101 | 4:bfdcf3da7cff | 48 | int i = 0; |
fabian101 | 2:6b913f93bc0b | 49 | |
fabian101 | 1:b9005f2aabf5 | 50 | const int margin = 100; // margin pulses for calibration |
fabian101 | 0:d935ea6f5c25 | 51 | |
fabian101 | 0:d935ea6f5c25 | 52 | void calibrate(); |
fabian101 | 1:b9005f2aabf5 | 53 | void calibratePower(); |
fabian101 | 0:d935ea6f5c25 | 54 | void run(); |
fabian101 | 1:b9005f2aabf5 | 55 | void initControllers(); |
fabian101 | 1:b9005f2aabf5 | 56 | void initMotors(); |
fabian101 | 0:d935ea6f5c25 | 57 | |
fabian101 | 6:a4440eec3652 | 58 | const float KcX = 0.0358; // THIS IS THE LIMIT FOR STABILITY |
fabian101 | 6:a4440eec3652 | 59 | const float KcY = 0.366; |
fabian101 | 1:b9005f2aabf5 | 60 | const float Ti = 0.0; |
fabian101 | 1:b9005f2aabf5 | 61 | const float Td = 0.0; |
fabian101 | 6:a4440eec3652 | 62 | const float RATE = 0.002; // rate = interval motors will be updateed |
fabian101 | 4:bfdcf3da7cff | 63 | |
fabian101 | 6:a4440eec3652 | 64 | int counter = 1; |
fabian101 | 2:6b913f93bc0b | 65 | |
fabian101 | 2:6b913f93bc0b | 66 | PwmOut X_Magnitude(D5); // bind to horizontal motor |
fabian101 | 2:6b913f93bc0b | 67 | DigitalOut X_Direction(D4); |
fabian101 | 6:a4440eec3652 | 68 | PID X_Controller(KcX, Ti, Td, RATE); |
fabian101 | 6:a4440eec3652 | 69 | QEI X_Motor(D13 ,D12 ,NC, 64 ,QEI::X4_ENCODING); |
fabian101 | 1:b9005f2aabf5 | 70 | |
fabian101 | 6:a4440eec3652 | 71 | PwmOut Y_Magnitude(D6); // bind to vertical motor |
fabian101 | 2:6b913f93bc0b | 72 | DigitalOut Y_Direction(D7); |
fabian101 | 6:a4440eec3652 | 73 | PID Y_Controller(KcY, Ti, Td, RATE); |
fabian101 | 6:a4440eec3652 | 74 | QEI Y_Motor(D15 ,D14 ,NC, 64 ,QEI::X4_ENCODING); |
fabian101 | 1:b9005f2aabf5 | 75 | |
fabian101 | 4:bfdcf3da7cff | 76 | //InterruptIn calibrateButton(D2); |
fabian101 | 4:bfdcf3da7cff | 77 | DigitalIn button(D2); |
fabian101 | 0:d935ea6f5c25 | 78 | Ticker motorControl; |
fabian101 | 4:bfdcf3da7cff | 79 | void setUpdate_flag(); |
fabian101 | 4:bfdcf3da7cff | 80 | volatile bool update_flag = false; |
fabian101 | 6:a4440eec3652 | 81 | const int CW = false; |
fabian101 | 2:6b913f93bc0b | 82 | |
fabian101 | 4:bfdcf3da7cff | 83 | DigitalOut led(LED_BLUE); |
fabian101 | 1:b9005f2aabf5 | 84 | |
fabian101 | 1:b9005f2aabf5 | 85 | void initMotors(){ |
fabian101 | 4:bfdcf3da7cff | 86 | X_Magnitude.period_ms(1); |
fabian101 | 2:6b913f93bc0b | 87 | X_Magnitude.write(0.0); |
fabian101 | 2:6b913f93bc0b | 88 | X_Direction.write(CW); |
fabian101 | 1:b9005f2aabf5 | 89 | |
fabian101 | 4:bfdcf3da7cff | 90 | Y_Magnitude.period_ms(1); |
fabian101 | 2:6b913f93bc0b | 91 | Y_Magnitude.write(0.0); |
fabian101 | 2:6b913f93bc0b | 92 | Y_Direction.write(CW); |
fabian101 | 6:a4440eec3652 | 93 | led = true; |
fabian101 | 1:b9005f2aabf5 | 94 | } |
fabian101 | 1:b9005f2aabf5 | 95 | void initControllers(){ |
fabian101 | 6:a4440eec3652 | 96 | X_Controller.setInputLimits(-maxSpeed,maxSpeed); // max output will be 10% |
fabian101 | 2:6b913f93bc0b | 97 | X_Controller.setOutputLimits(-1.0,1.0); |
fabian101 | 2:6b913f93bc0b | 98 | X_Controller.setBias(0); |
fabian101 | 6:a4440eec3652 | 99 | X_Controller.setMode(1); |
fabian101 | 1:b9005f2aabf5 | 100 | |
fabian101 | 6:a4440eec3652 | 101 | Y_Controller.setInputLimits(-maxSpeed,maxSpeed); // fix me |
fabian101 | 2:6b913f93bc0b | 102 | Y_Controller.setOutputLimits(-1.0,1.0); |
fabian101 | 2:6b913f93bc0b | 103 | Y_Controller.setBias(0); |
fabian101 | 6:a4440eec3652 | 104 | Y_Controller.setMode(1); |
fabian101 | 1:b9005f2aabf5 | 105 | } |