biorobotics group 19 , 2 november
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_controlv2 by
Controller.h@1:b9005f2aabf5, 2016-10-21 (annotated)
- Committer:
- fabian101
- Date:
- Fri Oct 21 21:32:20 2016 +0000
- Revision:
- 1:b9005f2aabf5
- Parent:
- 0:d935ea6f5c25
- Child:
- 2:6b913f93bc0b
21-10 11:32
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fabian101 | 0:d935ea6f5c25 | 1 | InterruptIn calibrateButton(D2); |
fabian101 | 1:b9005f2aabf5 | 2 | |
fabian101 | 1:b9005f2aabf5 | 3 | const double PI = 3.141592653589793; |
fabian101 | 1:b9005f2aabf5 | 4 | |
fabian101 | 1:b9005f2aabf5 | 5 | int x = 0; // pulsecount |
fabian101 | 1:b9005f2aabf5 | 6 | int x_max; // ??? physically measure |
fabian101 | 0:d935ea6f5c25 | 7 | int y = 0; |
fabian101 | 0:d935ea6f5c25 | 8 | int y_max; // ??? |
fabian101 | 1:b9005f2aabf5 | 9 | |
fabian101 | 1:b9005f2aabf5 | 10 | double rMax; // maxAmplitude emg signal |
fabian101 | 0:d935ea6f5c25 | 11 | double lMax; |
fabian101 | 0:d935ea6f5c25 | 12 | double uMax; |
fabian101 | 0:d935ea6f5c25 | 13 | double dMax; |
fabian101 | 1:b9005f2aabf5 | 14 | |
fabian101 | 1:b9005f2aabf5 | 15 | float minHorThreshold; // threshold emgsignal |
fabian101 | 1:b9005f2aabf5 | 16 | float minVerThreshold; |
fabian101 | 1:b9005f2aabf5 | 17 | |
fabian101 | 1:b9005f2aabf5 | 18 | float motorValueHor = 0; // requested speed |
fabian101 | 0:d935ea6f5c25 | 19 | float motorValueVer = 0; |
fabian101 | 0:d935ea6f5c25 | 20 | |
fabian101 | 1:b9005f2aabf5 | 21 | const float maxHorValue = 1.0f; // v1 in drawing, max speed cm/s |
fabian101 | 1:b9005f2aabf5 | 22 | const float maxVerValue = 1.0f; // |
fabian101 | 1:b9005f2aabf5 | 23 | |
fabian101 | 1:b9005f2aabf5 | 24 | const float maxSpeed = 1.0f; // cm/s |
fabian101 | 1:b9005f2aabf5 | 25 | |
fabian101 | 1:b9005f2aabf5 | 26 | const float R1 = 8.5; // radius of big driven gear in Cm |
fabian101 | 1:b9005f2aabf5 | 27 | const float RmHor = 0.5; // radius of small drive gear in Cm |
fabian101 | 1:b9005f2aabf5 | 28 | const float RmVer = 1.5915; // radius of pulley gear in Cm |
fabian101 | 1:b9005f2aabf5 | 29 | |
fabian101 | 1:b9005f2aabf5 | 30 | const float maxRotSpeedHor = maxSpeed/RmHor; // w_m = v1/r2 rad/s |
fabian101 | 1:b9005f2aabf5 | 31 | const float maxRotSpeedVer = maxSpeed/RmVer; // w_m = v1/r1 rad/s |
fabian101 | 1:b9005f2aabf5 | 32 | |
fabian101 | 1:b9005f2aabf5 | 33 | float currentX = 0; // CM |
fabian101 | 1:b9005f2aabf5 | 34 | float currentY = 0; |
fabian101 | 0:d935ea6f5c25 | 35 | |
fabian101 | 1:b9005f2aabf5 | 36 | float toX = 0; // CM |
fabian101 | 1:b9005f2aabf5 | 37 | float toY = 0; |
fabian101 | 0:d935ea6f5c25 | 38 | |
fabian101 | 1:b9005f2aabf5 | 39 | float currentXangle = 0; // degrees |
fabian101 | 1:b9005f2aabf5 | 40 | float currentYangle = 0; |
fabian101 | 1:b9005f2aabf5 | 41 | |
fabian101 | 1:b9005f2aabf5 | 42 | float toangleX = 0; // degrees |
fabian101 | 1:b9005f2aabf5 | 43 | float toangleY = 0; |
fabian101 | 1:b9005f2aabf5 | 44 | |
fabian101 | 1:b9005f2aabf5 | 45 | float Xvelrequest = 0; // degrees/sec |
fabian101 | 1:b9005f2aabf5 | 46 | float Yvelrequest = 0; |
fabian101 | 1:b9005f2aabf5 | 47 | |
fabian101 | 1:b9005f2aabf5 | 48 | float prevX = 0; // pulses |
fabian101 | 1:b9005f2aabf5 | 49 | float prevY = 0; |
fabian101 | 1:b9005f2aabf5 | 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 | 0:d935ea6f5c25 | 56 | double toMotorValue(double,double); |
fabian101 | 0:d935ea6f5c25 | 57 | void emergencyExit(); |
fabian101 | 0:d935ea6f5c25 | 58 | void motorOutput(double, bool); |
fabian101 | 1:b9005f2aabf5 | 59 | void initControllers(); |
fabian101 | 1:b9005f2aabf5 | 60 | void initMotors(); |
fabian101 | 0:d935ea6f5c25 | 61 | |
fabian101 | 0:d935ea6f5c25 | 62 | SigInterpreter signal = SigInterpreter(); |
fabian101 | 0:d935ea6f5c25 | 63 | PulseChecker pulses = PulseChecker(); |
fabian101 | 0:d935ea6f5c25 | 64 | |
fabian101 | 1:b9005f2aabf5 | 65 | const float Kc = 0.0; |
fabian101 | 1:b9005f2aabf5 | 66 | const float Ti = 0.0; |
fabian101 | 1:b9005f2aabf5 | 67 | const float Td = 0.0; |
fabian101 | 1:b9005f2aabf5 | 68 | float RATE = 1/1000; // ??? |
fabian101 | 1:b9005f2aabf5 | 69 | |
fabian101 | 1:b9005f2aabf5 | 70 | PwmOut horizontalMagnitude(D5); // bind to horizontal motor |
fabian101 | 1:b9005f2aabf5 | 71 | DigitalOut horizontalDirection(D4); |
fabian101 | 1:b9005f2aabf5 | 72 | PID horizontalController(Kc, Ti, Td, RATE); |
fabian101 | 1:b9005f2aabf5 | 73 | |
fabian101 | 1:b9005f2aabf5 | 74 | PwmOut verticalMagnitude(D6); // bind to vertical motor |
fabian101 | 1:b9005f2aabf5 | 75 | DigitalOut verticalDirection(D7); |
fabian101 | 1:b9005f2aabf5 | 76 | PID verticalController(Kc, Ti, Td, RATE); |
fabian101 | 1:b9005f2aabf5 | 77 | |
fabian101 | 0:d935ea6f5c25 | 78 | Ticker motorControl; |
fabian101 | 0:d935ea6f5c25 | 79 | void setFlag(); |
fabian101 | 0:d935ea6f5c25 | 80 | bool flag = true; |
fabian101 | 1:b9005f2aabf5 | 81 | |
fabian101 | 1:b9005f2aabf5 | 82 | void initMotors(){ |
fabian101 | 1:b9005f2aabf5 | 83 | horizontalMagnitude.period_us(50); |
fabian101 | 1:b9005f2aabf5 | 84 | horizontalMagnitude.write(0.0); |
fabian101 | 1:b9005f2aabf5 | 85 | horizontalDirection.write(1); |
fabian101 | 1:b9005f2aabf5 | 86 | |
fabian101 | 1:b9005f2aabf5 | 87 | verticalMagnitude.period_us(50); |
fabian101 | 1:b9005f2aabf5 | 88 | verticalMagnitude.write(0.0); |
fabian101 | 1:b9005f2aabf5 | 89 | verticalDirection.write(1); |
fabian101 | 1:b9005f2aabf5 | 90 | } |
fabian101 | 1:b9005f2aabf5 | 91 | void initControllers(){ |
fabian101 | 1:b9005f2aabf5 | 92 | horizontalController.setInputLimits(-90.0,90.0); // fix me |
fabian101 | 1:b9005f2aabf5 | 93 | horizontalController.setOutputLimits(-1.0,1.0); |
fabian101 | 1:b9005f2aabf5 | 94 | horizontalController.setBias(0); |
fabian101 | 1:b9005f2aabf5 | 95 | horizontalController.setMode(0); |
fabian101 | 1:b9005f2aabf5 | 96 | |
fabian101 | 1:b9005f2aabf5 | 97 | verticalController.setInputLimits(-90.0,90.0); // fix me |
fabian101 | 1:b9005f2aabf5 | 98 | verticalController.setOutputLimits(-1.0,1.0); |
fabian101 | 1:b9005f2aabf5 | 99 | verticalController.setBias(0); |
fabian101 | 1:b9005f2aabf5 | 100 | verticalController.setMode(0); |
fabian101 | 1:b9005f2aabf5 | 101 | } |