Irnti Nousi / Mbed 2 deprecated Robot_control_v4

Group19-Biorobotics

Dependencies:   HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed

Fork of Robot_control_v3 by Fabian van Hummel

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?

UserRevisionLine numberNew 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 }