Peter Knoben / Mbed 2 deprecated ShowIt_robot_control

Final Version

Dependencies:   AnglePosition2 Encoder FastPWM MovementClean PIDController Servo SignalNumberClean biquadFilter mbed

Fork of ShowItv2 by Peter Knoben

main.cpp@15:a73b5abd0696, 2017-11-07 (annotated)

Committer:
peterknoben
Date:
Tue Nov 07 09:31:19 2017 +0000
Revision:
15:a73b5abd0696
Parent:
14:a8a69fee3fad 
Final Version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
peterknoben 1:406519ff0f17 1 #include "AnglePosition.h"
peterknoben 1:406519ff0f17 2 #include "PIDControl.h"
peterknoben 3:c768a37620c9 3 #include "BiQuad.h"
peterknoben 3:c768a37620c9 4 #include "signalnumber.h"
peterknoben 4:e15fc329b88b 5 #include "Movement.h"
DBerendsen 0:5f4bc2d63814 6 #include "mbed.h"
DBerendsen 0:5f4bc2d63814 7 #include "encoder.h"
DBerendsen 0:5f4bc2d63814 8 #include "Servo.h"
peterknoben 1:406519ff0f17 9 #include "FastPWM.h"
DBerendsen 0:5f4bc2d63814 10
peterknoben 14:a8a69fee3fad 11 //This code is for the main Mbed
DBerendsen 0:5f4bc2d63814 12 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 13 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 14 //------------------------------------------------------------------------------
peterknoben 14:a8a69fee3fad 15 Ticker MyTickerController; //Declare Ticker for Motors
DBerendsen 11:28b5ec12a4b9 16 Ticker MyTickerServo; //Declare Ticker Servo control
peterknoben 3:c768a37620c9 17 Ticker MyTickerMean; //Declare Ticker Signalprocessing
peterknoben 14:a8a69fee3fad 18 Ticker MyTickerFilter; //Declare Ticker Filtering
peterknoben 3:c768a37620c9 19
peterknoben 14:a8a69fee3fad 20 InterruptIn But1(PTA4); //Declare button for emg calibration
peterknoben 14:a8a69fee3fad 21 InterruptIn Mode(PTC6); //Declare button for mode selection
peterknoben 3:c768a37620c9 22
peterknoben 1:406519ff0f17 23 AnglePosition Angle; //Declare Angle calculater
peterknoben 1:406519ff0f17 24 PIDControl PID; //Declare PID Controller
peterknoben 4:e15fc329b88b 25 SignalNumber SignalLeft; //Declare Signal determiner for Left arm
peterknoben 4:e15fc329b88b 26 SignalNumber SignalRight; //Declare Signal determiner for Right arm
peterknoben 14:a8a69fee3fad 27 Movement MoveLeft; //Declare Left Movement determiner
peterknoben 14:a8a69fee3fad 28 Movement MoveRight; //Declare Right movement determiner
peterknoben 3:c768a37620c9 29
peterknoben 4:e15fc329b88b 30 AnalogIn emg0( A0 ); //Set Inputpin for EMG 0 signal Left
peterknoben 4:e15fc329b88b 31 AnalogIn emg2( A2 ); //Set Inputpin for EMG 2 signal Right
peterknoben 14:a8a69fee3fad 32 DigitalOut Up( D9 ); //Set digital out for raising the pen off the board
peterknoben 14:a8a69fee3fad 33 DigitalOut Down( D8 ); //Set digital out for lowering the pen off the board
peterknoben 14:a8a69fee3fad 34
peterknoben 14:a8a69fee3fad 35 //Setting LEDs
peterknoben 14:a8a69fee3fad 36 DigitalOut Led_red(LED_RED);
peterknoben 4:e15fc329b88b 37 DigitalOut Led_green(LED_GREEN);
peterknoben 4:e15fc329b88b 38 DigitalOut Led_blue(LED_BLUE);
DBerendsen 12:0765ea2c4c85 39 DigitalOut Led_calibration(D2);
peterknoben 1:406519ff0f17 40
peterknoben 1:406519ff0f17 41 const float CONTROLLER_TS = 0.02; //Motor frequency
peterknoben 14:a8a69fee3fad 42 const float MEAN_TS = 0.002; //Moving average frequency
peterknoben 14:a8a69fee3fad 43 const float FILTER_TS = 1e3; //Filter frequency
peterknoben 14:a8a69fee3fad 44 const float SERVO_TS = 0.01; //Servo frequency
peterknoben 4:e15fc329b88b 45
peterknoben 14:a8a69fee3fad 46 void setled(){
peterknoben 14:a8a69fee3fad 47 Led_red=1; Led_green=1; Led_blue=1; Led_calibration=0;
peterknoben 14:a8a69fee3fad 48 }
peterknoben 3:c768a37620c9 49 //------------------------------------------------------------------------------
peterknoben 3:c768a37620c9 50 //-----------------------------EMG Signals--------------------------------------
peterknoben 3:c768a37620c9 51 //------------------------------------------------------------------------------
peterknoben 14:a8a69fee3fad 52
peterknoben 4:e15fc329b88b 53 // Filtering the signal and getting the usefull information out of it.
peterknoben 14:a8a69fee3fad 54 const int n = 500; //Window size for the mean value, also adjust in signalnumber.cpp
peterknoben 14:a8a69fee3fad 55 const int action =100; //Number of same mean values to change the signalnumber
peterknoben 14:a8a69fee3fad 56 const int m = 500; //Number of samples for calibration
peterknoben 14:a8a69fee3fad 57 int SpeedLeft, SpeedRight; //Strength of the muscles
peterknoben 14:a8a69fee3fad 58 float emg_offsetLeft, emg_offsetRight; //Calibration offsets from zero to one for the right arm
peterknoben 14:a8a69fee3fad 59 float kLeft, kRight; //Scaling factors
peterknoben 14:a8a69fee3fad 60 float Signal_filteredLeft, Signal_filteredRight; //Variables to store the filterd signals
peterknoben 3:c768a37620c9 61
peterknoben 4:e15fc329b88b 62 //BiQuad filter variables
peterknoben 6:edd30e11f3c7 63 BiQuad LP1L( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
peterknoben 6:edd30e11f3c7 64 BiQuad HP2L( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
peterknoben 6:edd30e11f3c7 65 BiQuad NO3L( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad
peterknoben 6:edd30e11f3c7 66 BiQuad LP1R( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
peterknoben 6:edd30e11f3c7 67 BiQuad HP2R( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
peterknoben 6:edd30e11f3c7 68 BiQuad NO3R( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad
peterknoben 6:edd30e11f3c7 69 BiQuadChain BiQuad_filter_Left;
peterknoben 6:edd30e11f3c7 70 BiQuadChain BiQuad_filter_Right;
peterknoben 3:c768a37620c9 71
peterknoben 14:a8a69fee3fad 72 //Filters-----------------------------------------------------------------------
peterknoben 14:a8a69fee3fad 73
peterknoben 14:a8a69fee3fad 74 // Filter for the left signal
peterknoben 7:105e3ae0fb19 75 float FilterLeft(float input){
peterknoben 7:105e3ae0fb19 76 float Signal_filtered= BiQuad_filter_Left.step(input);
peterknoben 7:105e3ae0fb19 77 return Signal_filtered;
peterknoben 7:105e3ae0fb19 78 }
peterknoben 14:a8a69fee3fad 79 // Filter for the right signal
peterknoben 7:105e3ae0fb19 80 float FilterRight(float input){
peterknoben 7:105e3ae0fb19 81 float Signal_filtered= BiQuad_filter_Right.step(input);
peterknoben 7:105e3ae0fb19 82 return Signal_filtered;
peterknoben 7:105e3ae0fb19 83 }
peterknoben 5:b4abbd3c513c 84
peterknoben 5:b4abbd3c513c 85 //Calibration-------------------------------------------------------------------
peterknoben 14:a8a69fee3fad 86
peterknoben 14:a8a69fee3fad 87 // Calibrating the EMG signals. This function needs to be ativated when the
peterknoben 14:a8a69fee3fad 88 // muscles are flexed for over a second
peterknoben 14:a8a69fee3fad 89 void calibration(){
peterknoben 14:a8a69fee3fad 90 emg_offsetLeft = SignalLeft.calibrate(m, FilterLeft(emg0));
peterknoben 14:a8a69fee3fad 91 emg_offsetRight = SignalRight.calibrate(m, FilterRight(emg2));
peterknoben 14:a8a69fee3fad 92 //Setting the emg scaling constants to reed between zero and one.
peterknoben 14:a8a69fee3fad 93 kLeft = 1/emg_offsetLeft;
peterknoben 14:a8a69fee3fad 94 kRight = 1/emg_offsetRight;
peterknoben 14:a8a69fee3fad 95 //Set led to Green to indicate if calibration is done
peterknoben 14:a8a69fee3fad 96 Led_calibration = 1;
peterknoben 5:b4abbd3c513c 97 }
peterknoben 8:24f6744d47b2 98
peterknoben 14:a8a69fee3fad 99 //Speedcontrol------------------------------------------------------------------
peterknoben 14:a8a69fee3fad 100 //Determine the signalnumbers (i.e. speed) based on the strenght of the EMG signals
peterknoben 14:a8a69fee3fad 101 void Speedcontrol(){
peterknoben 14:a8a69fee3fad 102 //Left Signals
peterknoben 14:a8a69fee3fad 103 Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);//*kLeft
peterknoben 14:a8a69fee3fad 104 SpeedLeft = SignalLeft.getspeedLeft(n, action, Signal_filteredLeft);
peterknoben 14:a8a69fee3fad 105 //Right Signals
peterknoben 14:a8a69fee3fad 106 Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight
peterknoben 14:a8a69fee3fad 107 SpeedRight = SignalRight.getspeedRight(n, action, Signal_filteredRight);
peterknoben 3:c768a37620c9 108 }
peterknoben 3:c768a37620c9 109
peterknoben 5:b4abbd3c513c 110 //------------------------------------------------------------------------------
peterknoben 5:b4abbd3c513c 111 //---------------------------------Servo----------------------------------------
peterknoben 5:b4abbd3c513c 112 //------------------------------------------------------------------------------
peterknoben 14:a8a69fee3fad 113 //Sending a pulse to the other Mbed to raise the pen
DBerendsen 11:28b5ec12a4b9 114 void up(){
peterknoben 14:a8a69fee3fad 115 Up = 1; Up = 0; Up = 1;
DBerendsen 11:28b5ec12a4b9 116 }
peterknoben 14:a8a69fee3fad 117 //Sending a pulse to the other Mbed to lower the pen
DBerendsen 11:28b5ec12a4b9 118 void down(){
peterknoben 14:a8a69fee3fad 119 Down = 1; Down = 0; Down = 1;
DBerendsen 11:28b5ec12a4b9 120 }
DBerendsen 11:28b5ec12a4b9 121
peterknoben 14:a8a69fee3fad 122 //Setting the begin mode to zero
DBerendsen 11:28b5ec12a4b9 123 int mode =0;
peterknoben 14:a8a69fee3fad 124 // Enable the servo function when mode 3 or mode 6 is activated.
peterknoben 14:a8a69fee3fad 125 // If the mean value for the Left arm stays long enough high the servo will raise
peterknoben 14:a8a69fee3fad 126 // If the mean value for the Right arm stays long enough high the servo will lower
peterknoben 5:b4abbd3c513c 127 void servo(){
DBerendsen 11:28b5ec12a4b9 128 if(mode==3||mode==6){
peterknoben 14:a8a69fee3fad 129 Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);
peterknoben 14:a8a69fee3fad 130 Signal_filteredRight = fabs(FilterRight(emg2)*kRight);
peterknoben 14:a8a69fee3fad 131 SpeedLeft = SignalLeft.getspeedLeft(n, action, Signal_filteredLeft);
peterknoben 14:a8a69fee3fad 132 SpeedRight = SignalRight.getspeedRight(n, action, Signal_filteredRight);
peterknoben 14:a8a69fee3fad 133 if (SpeedLeft == 2){
DBerendsen 11:28b5ec12a4b9 134 up();
DBerendsen 11:28b5ec12a4b9 135 }
peterknoben 14:a8a69fee3fad 136 if (SpeedRight == 2){
DBerendsen 11:28b5ec12a4b9 137 down();
DBerendsen 11:28b5ec12a4b9 138 }
peterknoben 5:b4abbd3c513c 139 }
peterknoben 5:b4abbd3c513c 140 }
peterknoben 5:b4abbd3c513c 141 //------------------------------------------------------------------------------
peterknoben 5:b4abbd3c513c 142 //---------------------------Mode selection-------------------------------------
peterknoben 5:b4abbd3c513c 143 //------------------------------------------------------------------------------
peterknoben 5:b4abbd3c513c 144
peterknoben 14:a8a69fee3fad 145 //Function is called when the mode button is pressed and switches modes and led indication
peterknoben 5:b4abbd3c513c 146 void mode_selection(){
peterknoben 14:a8a69fee3fad 147 //Switching to the next mode. (Total of 6 modes)
peterknoben 14:a8a69fee3fad 148 if(mode == 6){
peterknoben 5:b4abbd3c513c 149 mode=1;
peterknoben 5:b4abbd3c513c 150 }
peterknoben 5:b4abbd3c513c 151 else{
peterknoben 5:b4abbd3c513c 152 mode++;
peterknoben 14:a8a69fee3fad 153 }
peterknoben 14:a8a69fee3fad 154 //Setting the LED colour to indicate the operating mode
DBerendsen 12:0765ea2c4c85 155 if((mode==3) || (mode==6)) {
peterknoben 14:a8a69fee3fad 156 Led_red = 0, Led_green = 0, Led_blue = 0; //Set Led Mode to White
DBerendsen 10:614050a50c2f 157 }
DBerendsen 10:614050a50c2f 158 else if (mode==1) {
peterknoben 14:a8a69fee3fad 159 Led_red = 0, Led_green = 1, Led_blue = 1; //Set Led Mode to Red
DBerendsen 10:614050a50c2f 160 }
DBerendsen 10:614050a50c2f 161 else if (mode==2) {
peterknoben 14:a8a69fee3fad 162 Led_red = 1, Led_green = 0, Led_blue = 1; //Set Led Mode to Green
DBerendsen 10:614050a50c2f 163 }
DBerendsen 10:614050a50c2f 164 else if (mode==4) {
peterknoben 14:a8a69fee3fad 165 Led_red = 1, Led_green = 1, Led_blue = 0; //Set Led Mode to Blue
DBerendsen 10:614050a50c2f 166 }
DBerendsen 10:614050a50c2f 167 else if (mode==5) {
peterknoben 14:a8a69fee3fad 168 Led_red = 0, Led_green = 0, Led_blue = 1; //Set Led Mode to Yellow
DBerendsen 12:0765ea2c4c85 169 }
peterknoben 5:b4abbd3c513c 170 }
peterknoben 1:406519ff0f17 171 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 172 //-------------------------Kinematic Constants----------------------------------
peterknoben 1:406519ff0f17 173 //------------------------------------------------------------------------------
peterknoben 4:e15fc329b88b 174 const double RAD_PER_PULSE = 0.00074799825*2; //Number of radials per pulse from the encoder
peterknoben 4:e15fc329b88b 175 const double PI = 3.14159265358979323846; //Pi
DBerendsen 11:28b5ec12a4b9 176 const float max_rangex = 650.0, max_rangey = 450.0; //Max range on the y axis
peterknoben 8:24f6744d47b2 177 const float x_offset = 156.15, y_offset = -76.97; //Starting positions from the shoulder joint
DBerendsen 10:614050a50c2f 178 const float alpha_offset = -0.4114;
DBerendsen 10:614050a50c2f 179 const float beta_offset = 0.0486;
peterknoben 8:24f6744d47b2 180 const float L1 = 450.0, L2 = 490.0; //Length of the bodies
peterknoben 5:b4abbd3c513c 181
DBerendsen 0:5f4bc2d63814 182 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 183 //--------------------------------Motor1----------------------------------------
DBerendsen 0:5f4bc2d63814 184 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 185 FastPWM motor1(D5);
DBerendsen 0:5f4bc2d63814 186 DigitalOut motor1DirectionPin(D4);
DBerendsen 0:5f4bc2d63814 187 DigitalIn ENC2A(D12);
DBerendsen 0:5f4bc2d63814 188 DigitalIn ENC2B(D13);
DBerendsen 0:5f4bc2d63814 189 Encoder encoder1(D13,D12);
peterknoben 1:406519ff0f17 190 const float MOTOR1_KP = 40.0;
peterknoben 1:406519ff0f17 191 const float MOTOR1_KI = 0.0;
peterknoben 1:406519ff0f17 192 const float MOTOR1_KD = 15.0;
peterknoben 5:b4abbd3c513c 193 double M1_v1 = 0.0, M1_v2 = 0.0; //Calculation values
DBerendsen 0:5f4bc2d63814 194 const double motor1_gain = 2*PI;
peterknoben 1:406519ff0f17 195 const float M1_N = 0.5;
peterknoben 8:24f6744d47b2 196 float position_math_l =0, position_math_r=0;
DBerendsen 0:5f4bc2d63814 197
DBerendsen 12:0765ea2c4c85 198 void motor1_control(){
peterknoben 15:a73b5abd0696 199 float reference_beta = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math_l, position_math_r);
peterknoben 15:a73b5abd0696 200 float position_beta = RAD_PER_PULSE * encoder1.getPosition();
peterknoben 15:a73b5abd0696 201 float error_beta = reference_beta-position_beta;
peterknoben 15:a73b5abd0696 202 float magnitude1 = PID.get(error_beta, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain;
DBerendsen 10:614050a50c2f 203 //Determine Motor Magnitude
DBerendsen 10:614050a50c2f 204 if (fabs(magnitude1)>1) {
DBerendsen 10:614050a50c2f 205 motor1 = 1;
DBerendsen 10:614050a50c2f 206 }
DBerendsen 10:614050a50c2f 207 else {
DBerendsen 10:614050a50c2f 208 motor1 = fabs(magnitude1);
DBerendsen 10:614050a50c2f 209 }
peterknoben 1:406519ff0f17 210 // Determine Motor Direction
peterknoben 1:406519ff0f17 211 if (magnitude1 < 0){
DBerendsen 0:5f4bc2d63814 212 motor1DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 213 }
DBerendsen 0:5f4bc2d63814 214 else{
DBerendsen 0:5f4bc2d63814 215 motor1DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 216 }
DBerendsen 0:5f4bc2d63814 217 }
DBerendsen 0:5f4bc2d63814 218 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 219 //--------------------------------Motor2----------------------------------------
DBerendsen 0:5f4bc2d63814 220 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 221 FastPWM motor2(D6);
DBerendsen 0:5f4bc2d63814 222 DigitalOut motor2DirectionPin(D7);
DBerendsen 0:5f4bc2d63814 223 DigitalIn ENC1A(D10);
DBerendsen 0:5f4bc2d63814 224 DigitalIn ENC1B(D11);
DBerendsen 0:5f4bc2d63814 225 Encoder encoder2(D10,D11);
peterknoben 1:406519ff0f17 226 const float MOTOR2_KP = 60.0;
peterknoben 1:406519ff0f17 227 const float MOTOR2_KI = 0.0;
peterknoben 1:406519ff0f17 228 const float MOTOR2_KD = 15.0;
peterknoben 5:b4abbd3c513c 229 double M2_v1 = 0.0, M2_v2 = 0.0; //Calculation values
DBerendsen 0:5f4bc2d63814 230 const double motor2_gain = 2*PI;
peterknoben 1:406519ff0f17 231 const float M2_N = 0.5;
DBerendsen 0:5f4bc2d63814 232
DBerendsen 12:0765ea2c4c85 233 void motor2_control(){
peterknoben 15:a73b5abd0696 234 float reference_alpha = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, position_math_l, position_math_r);
peterknoben 15:a73b5abd0696 235 float position_alpha = RAD_PER_PULSE * -encoder2.getPosition();
peterknoben 15:a73b5abd0696 236 float error_alpha = reference_alpha-position_alpha;
peterknoben 15:a73b5abd0696 237 float magnitude2 = PID.get(error_alpha, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain;
DBerendsen 10:614050a50c2f 238 //Determine Motor Magnitude
DBerendsen 10:614050a50c2f 239 if (fabs(magnitude2)>1) {
DBerendsen 10:614050a50c2f 240 motor2 = 1;
DBerendsen 10:614050a50c2f 241 }
DBerendsen 10:614050a50c2f 242 else {
DBerendsen 10:614050a50c2f 243 motor2 = fabs(magnitude2);
DBerendsen 10:614050a50c2f 244 }
peterknoben 1:406519ff0f17 245 //Determine Motor Direction
DBerendsen 0:5f4bc2d63814 246 if (magnitude2 > 0){
DBerendsen 0:5f4bc2d63814 247 motor2DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 248 }
DBerendsen 0:5f4bc2d63814 249 else{
DBerendsen 0:5f4bc2d63814 250 motor2DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 251 }
DBerendsen 0:5f4bc2d63814 252 }
peterknoben 8:24f6744d47b2 253 //------------------------------------------------------------------------------
peterknoben 8:24f6744d47b2 254 //----------------------------Motor controller----------------------------------
peterknoben 8:24f6744d47b2 255 //------------------------------------------------------------------------------
peterknoben 14:a8a69fee3fad 256 int direction_l, direction_r; //Variables to store the directions
peterknoben 14:a8a69fee3fad 257 float magnitude1, magnitude2; //Variables to store the magnitude signal
peterknoben 8:24f6744d47b2 258
peterknoben 5:b4abbd3c513c 259 void motor_control(){
DBerendsen 10:614050a50c2f 260 direction_l = MoveLeft.getdirectionLeft(mode); //x-direction
DBerendsen 10:614050a50c2f 261 direction_r = MoveRight.getdirectionRight(mode); //y-direction
peterknoben 14:a8a69fee3fad 262 position_math_l= MoveLeft.getpositionLeft(SpeedLeft, mode, max_rangex); //x-direction
peterknoben 14:a8a69fee3fad 263 position_math_r= MoveRight.getpositionRight(SpeedRight, mode, max_rangey); //y-direction
peterknoben 14:a8a69fee3fad 264 motor1_control();
peterknoben 14:a8a69fee3fad 265 motor2_control();
peterknoben 5:b4abbd3c513c 266 }
DBerendsen 11:28b5ec12a4b9 267 //------------------------------------------------------------------------------
peterknoben 14:a8a69fee3fad 268 //-----------------------------Filter Update------------------------------------
DBerendsen 0:5f4bc2d63814 269 //------------------------------------------------------------------------------
peterknoben 8:24f6744d47b2 270
peterknoben 14:a8a69fee3fad 271 //Updating Filter values
DBerendsen 12:0765ea2c4c85 272 void FilterUpdate(){
DBerendsen 12:0765ea2c4c85 273 FilterLeft(emg0.read());
DBerendsen 12:0765ea2c4c85 274 FilterRight(emg2.read());
DBerendsen 12:0765ea2c4c85 275 SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())));
DBerendsen 12:0765ea2c4c85 276 SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())));
peterknoben 8:24f6744d47b2 277 }
peterknoben 8:24f6744d47b2 278 //------------------------------------------------------------------------------
peterknoben 8:24f6744d47b2 279 //------------------------------Main--------------------------------------------
DBerendsen 0:5f4bc2d63814 280 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 281
DBerendsen 0:5f4bc2d63814 282 int main(){
peterknoben 4:e15fc329b88b 283 setled();
peterknoben 14:a8a69fee3fad 284
peterknoben 14:a8a69fee3fad 285 //Creating Filters
peterknoben 6:edd30e11f3c7 286 BiQuad_filter_Left.add( &LP1L ).add( &HP2L ).add( &NO3L);
peterknoben 6:edd30e11f3c7 287 BiQuad_filter_Right.add( &LP1R ).add( &HP2R ).add( &NO3R);
peterknoben 14:a8a69fee3fad 288
peterknoben 14:a8a69fee3fad 289 //Button detection
peterknoben 14:a8a69fee3fad 290 But1.rise(&calibration);
peterknoben 6:edd30e11f3c7 291 Mode.rise(&mode_selection);
peterknoben 14:a8a69fee3fad 292
peterknoben 14:a8a69fee3fad 293 //Setting motor periods
peterknoben 5:b4abbd3c513c 294 motor1.period(0.0001f); motor2.period(0.0001f);
peterknoben 14:a8a69fee3fad 295
peterknoben 14:a8a69fee3fad 296 //Tickers
peterknoben 14:a8a69fee3fad 297 MyTickerController.attach(&motor_control, CONTROLLER_TS);
peterknoben 14:a8a69fee3fad 298 MyTickerMean.attach(&Speedcontrol, MEAN_TS);
peterknoben 14:a8a69fee3fad 299 MyTickerServo.attach(&servo, SERVO_TS);
peterknoben 14:a8a69fee3fad 300 MyTickerFilter.attach_us(&FilterUpdate, FILTER_TS);
peterknoben 8:24f6744d47b2 301
peterknoben 5:b4abbd3c513c 302 while(1) {
peterknoben 5:b4abbd3c513c 303 }
peterknoben 14:a8a69fee3fad 304 }