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@11:28b5ec12a4b9, 2017-11-02 (annotated)

Committer:
DBerendsen
Date:
Thu Nov 02 11:39:59 2017 +0000
Revision:
11:28b5ec12a4b9
Parent:
10:614050a50c2f
Child:
12:0765ea2c4c85
mhg

Who changed what in which revision?

UserRevisionLine numberNew contents of line
peterknoben 3:c768a37620c9 1 #include "MODSERIAL.h"
peterknoben 1:406519ff0f17 2 #include "AnglePosition.h"
peterknoben 1:406519ff0f17 3 #include "PIDControl.h"
peterknoben 3:c768a37620c9 4 #include "BiQuad.h"
peterknoben 3:c768a37620c9 5 #include "signalnumber.h"
peterknoben 4:e15fc329b88b 6 #include "Movement.h"
DBerendsen 0:5f4bc2d63814 7 #include "mbed.h"
DBerendsen 0:5f4bc2d63814 8 #include "encoder.h"
DBerendsen 0:5f4bc2d63814 9 #include "Servo.h"
peterknoben 1:406519ff0f17 10 #include "FastPWM.h"
peterknoben 8:24f6744d47b2 11 #include "HIDScope.h"
DBerendsen 0:5f4bc2d63814 12
peterknoben 4:e15fc329b88b 13 //This code is for Mbed 2
DBerendsen 0:5f4bc2d63814 14 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 15 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 16 //------------------------------------------------------------------------------
peterknoben 3:c768a37620c9 17 MODSERIAL pc(USBTX, USBRX); //Establish connection
peterknoben 8:24f6744d47b2 18 Ticker MyControllerTicker; //Declare Ticker for Motors
DBerendsen 11:28b5ec12a4b9 19 Ticker MyTickerServo; //Declare Ticker Servo control
peterknoben 3:c768a37620c9 20 Ticker MyTickerMean; //Declare Ticker Signalprocessing
peterknoben 3:c768a37620c9 21
peterknoben 8:24f6744d47b2 22 InterruptIn But1(PTA4); //Declare button for min calibration
peterknoben 8:24f6744d47b2 23 InterruptIn Mode(PTC6); //Declare button for max calibration
peterknoben 3:c768a37620c9 24
peterknoben 1:406519ff0f17 25 AnglePosition Angle; //Declare Angle calculater
peterknoben 1:406519ff0f17 26 PIDControl PID; //Declare PID Controller
peterknoben 4:e15fc329b88b 27 SignalNumber SignalLeft; //Declare Signal determiner for Left arm
peterknoben 4:e15fc329b88b 28 SignalNumber SignalRight; //Declare Signal determiner for Right arm
peterknoben 5:b4abbd3c513c 29 SignalNumber SignalMode;
peterknoben 4:e15fc329b88b 30 Movement MoveLeft; //Declare Movement determiner
peterknoben 4:e15fc329b88b 31 Movement MoveRight;
peterknoben 3:c768a37620c9 32
peterknoben 4:e15fc329b88b 33 AnalogIn emg0( A0 ); //Set Inputpin for EMG 0 signal Left
peterknoben 4:e15fc329b88b 34 AnalogIn emg2( A2 ); //Set Inputpin for EMG 2 signal Right
peterknoben 4:e15fc329b88b 35 AnalogIn emg4( A4 ); //Set Inputpin for EMG 4 signal Mode
peterknoben 5:b4abbd3c513c 36 DigitalOut Up( D9 ); //Set digital in for mode selection
peterknoben 5:b4abbd3c513c 37 DigitalOut Down( D8 );
peterknoben 4:e15fc329b88b 38 DigitalOut Led_red(LED_RED);
peterknoben 4:e15fc329b88b 39 DigitalOut Led_green(LED_GREEN);
peterknoben 4:e15fc329b88b 40 DigitalOut Led_blue(LED_BLUE);
peterknoben 1:406519ff0f17 41
peterknoben 1:406519ff0f17 42 const float CONTROLLER_TS = 0.02; //Motor frequency
peterknoben 4:e15fc329b88b 43 const float MEAN_TS = 0.002; //Filter frequency
peterknoben 4:e15fc329b88b 44
peterknoben 3:c768a37620c9 45 //------------------------------------------------------------------------------
peterknoben 3:c768a37620c9 46 //-----------------------------EMG Signals--------------------------------------
peterknoben 3:c768a37620c9 47 //------------------------------------------------------------------------------
peterknoben 4:e15fc329b88b 48 // Filtering the signal and getting the usefull information out of it.
DBerendsen 10:614050a50c2f 49 const int n = 500; //Window size for the mean value, also adjust in signalnumber.cpp
peterknoben 8:24f6744d47b2 50 const int action =100; //Number of same mean values to change the signalnumber
DBerendsen 10:614050a50c2f 51 const int m = 500; //Number of samples for calibration
peterknoben 5:b4abbd3c513c 52 int CaseLeft, CaseRight, CaseMode; //Strength of the muscles
peterknoben 5:b4abbd3c513c 53 float emg_offsetLeft, emg_offsetmaxLeft; //Calibration offsets from zero to one for the left arm
peterknoben 5:b4abbd3c513c 54 float emg_offsetRight, emg_offsetmaxRight; //Calibration offsets from zero to one for the right arm
peterknoben 5:b4abbd3c513c 55 float emg_offsetMode, emg_offsetmaxMode;
peterknoben 5:b4abbd3c513c 56 float mean_value_left, mean_value_right, mean_value_mode; //Mean value of the filtered system
peterknoben 5:b4abbd3c513c 57 float kLeft, kRight, kMode; //Scaling factors
peterknoben 6:edd30e11f3c7 58 float Signal_filteredLeft, Signal_filteredRight, Signal_filteredMode;
peterknoben 3:c768a37620c9 59
peterknoben 4:e15fc329b88b 60 //BiQuad filter variables
peterknoben 6:edd30e11f3c7 61 BiQuad LP1L( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
peterknoben 6:edd30e11f3c7 62 BiQuad HP2L( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
peterknoben 6:edd30e11f3c7 63 BiQuad NO3L( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad
peterknoben 6:edd30e11f3c7 64 BiQuad LP1R( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
peterknoben 6:edd30e11f3c7 65 BiQuad HP2R( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
peterknoben 6:edd30e11f3c7 66 BiQuad NO3R( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad
peterknoben 6:edd30e11f3c7 67 BiQuad LP1M( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
peterknoben 6:edd30e11f3c7 68 BiQuad HP2M( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
peterknoben 6:edd30e11f3c7 69 BiQuad NO3M( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad
peterknoben 6:edd30e11f3c7 70 BiQuadChain BiQuad_filter_Left;
peterknoben 6:edd30e11f3c7 71 BiQuadChain BiQuad_filter_Right;
peterknoben 6:edd30e11f3c7 72 BiQuadChain BiQuad_filter_Mode;
peterknoben 3:c768a37620c9 73
peterknoben 7:105e3ae0fb19 74 float FilterLeft(float input){
peterknoben 7:105e3ae0fb19 75 float Signal_filtered= BiQuad_filter_Left.step(input);
peterknoben 7:105e3ae0fb19 76 return Signal_filtered;
peterknoben 7:105e3ae0fb19 77 }
peterknoben 7:105e3ae0fb19 78 float FilterRight(float input){
peterknoben 7:105e3ae0fb19 79 float Signal_filtered= BiQuad_filter_Right.step(input);
peterknoben 7:105e3ae0fb19 80 return Signal_filtered;
peterknoben 7:105e3ae0fb19 81 }
peterknoben 7:105e3ae0fb19 82 float FilterMode(float input){
peterknoben 7:105e3ae0fb19 83 float Signal_filtered= BiQuad_filter_Mode.step(input);
peterknoben 7:105e3ae0fb19 84 return Signal_filtered;
peterknoben 7:105e3ae0fb19 85 }
peterknoben 5:b4abbd3c513c 86
peterknoben 5:b4abbd3c513c 87 //Calibration-------------------------------------------------------------------
peterknoben 4:e15fc329b88b 88 void setled(){
peterknoben 5:b4abbd3c513c 89 Led_red=0; Led_green=1; Led_blue=1;
peterknoben 5:b4abbd3c513c 90 }
peterknoben 8:24f6744d47b2 91
peterknoben 5:b4abbd3c513c 92 // One calibration
peterknoben 5:b4abbd3c513c 93 void maxcalibration(){
peterknoben 5:b4abbd3c513c 94 pc.printf("start max calibration \r\n");
peterknoben 8:24f6744d47b2 95 emg_offsetmaxLeft = SignalLeft.calibrate(m, FilterLeft(emg0));
peterknoben 8:24f6744d47b2 96 emg_offsetmaxRight = SignalRight.calibrate(m, FilterRight(emg2));
peterknoben 8:24f6744d47b2 97 emg_offsetmaxMode = SignalMode.calibrate(m, FilterMode(emg4));
peterknoben 5:b4abbd3c513c 98 kLeft = 1/emg_offsetmaxLeft;
peterknoben 5:b4abbd3c513c 99 kRight = 1/emg_offsetmaxRight;
peterknoben 5:b4abbd3c513c 100 kMode = 1/emg_offsetmaxMode;
peterknoben 5:b4abbd3c513c 101 Led_red=1; //Set led to Green
peterknoben 3:c768a37620c9 102 }
peterknoben 3:c768a37620c9 103
peterknoben 5:b4abbd3c513c 104 //------------------------------------------------------------------------------
peterknoben 5:b4abbd3c513c 105 //---------------------------------Servo----------------------------------------
peterknoben 5:b4abbd3c513c 106 //------------------------------------------------------------------------------
DBerendsen 11:28b5ec12a4b9 107 void up(){
DBerendsen 11:28b5ec12a4b9 108 Up = 1;
DBerendsen 11:28b5ec12a4b9 109 Up = 0;
DBerendsen 11:28b5ec12a4b9 110 Up = 1;
DBerendsen 11:28b5ec12a4b9 111 }
DBerendsen 11:28b5ec12a4b9 112 void down(){
DBerendsen 11:28b5ec12a4b9 113 Down = 1;
DBerendsen 11:28b5ec12a4b9 114 Down = 0;
DBerendsen 11:28b5ec12a4b9 115 Down = 1;
DBerendsen 11:28b5ec12a4b9 116 }
DBerendsen 11:28b5ec12a4b9 117
DBerendsen 11:28b5ec12a4b9 118 int mode =0;
peterknoben 5:b4abbd3c513c 119 void servo(){
DBerendsen 11:28b5ec12a4b9 120 if(mode==3||mode==6){
DBerendsen 11:28b5ec12a4b9 121 Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);//*kLeft
DBerendsen 11:28b5ec12a4b9 122 CaseLeft = SignalLeft.getnumberLeft(n, action, Signal_filteredLeft);
DBerendsen 11:28b5ec12a4b9 123 Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight
DBerendsen 11:28b5ec12a4b9 124 CaseRight = SignalRight.getnumberRight(n, action, Signal_filteredRight);
DBerendsen 11:28b5ec12a4b9 125 if (CaseLeft == 2){
DBerendsen 11:28b5ec12a4b9 126 up();
DBerendsen 11:28b5ec12a4b9 127 }
DBerendsen 11:28b5ec12a4b9 128 else if (CaseRight == 2){
DBerendsen 11:28b5ec12a4b9 129 down();
DBerendsen 11:28b5ec12a4b9 130 }
peterknoben 5:b4abbd3c513c 131 }
peterknoben 5:b4abbd3c513c 132 }
DBerendsen 10:614050a50c2f 133
peterknoben 5:b4abbd3c513c 134
peterknoben 5:b4abbd3c513c 135 //------------------------------------------------------------------------------
peterknoben 5:b4abbd3c513c 136 //---------------------------Mode selection-------------------------------------
peterknoben 5:b4abbd3c513c 137 //------------------------------------------------------------------------------
peterknoben 5:b4abbd3c513c 138
peterknoben 5:b4abbd3c513c 139 //Recieving mode selection from EMG mode signal
peterknoben 5:b4abbd3c513c 140 void mode_selection(){
peterknoben 5:b4abbd3c513c 141 if(mode ==6){
peterknoben 5:b4abbd3c513c 142 mode=1;
peterknoben 5:b4abbd3c513c 143 }
peterknoben 5:b4abbd3c513c 144 else{
peterknoben 5:b4abbd3c513c 145 mode++;
DBerendsen 11:28b5ec12a4b9 146 }
peterknoben 5:b4abbd3c513c 147 pc.printf("\r\n mode = %i \r\n", mode);
DBerendsen 10:614050a50c2f 148
DBerendsen 10:614050a50c2f 149
DBerendsen 10:614050a50c2f 150 /*if((mode==3) || (mode==6)) {
DBerendsen 10:614050a50c2f 151 Led_red = 0;
DBerendsen 10:614050a50c2f 152 Led_green = 0;
DBerendsen 10:614050a50c2f 153 Led_blue = 0;
DBerendsen 10:614050a50c2f 154 }
DBerendsen 10:614050a50c2f 155 else if (mode==1) {
DBerendsen 10:614050a50c2f 156 Led_red = 0;
DBerendsen 10:614050a50c2f 157 Led_green = 1;
DBerendsen 10:614050a50c2f 158 Led_blue = 1;
DBerendsen 10:614050a50c2f 159 }
DBerendsen 10:614050a50c2f 160 else if (mode==2) {
DBerendsen 10:614050a50c2f 161 Led_red = 1;
DBerendsen 10:614050a50c2f 162 Led_green = 0;
DBerendsen 10:614050a50c2f 163 Led_blue = 1;
DBerendsen 10:614050a50c2f 164 }
DBerendsen 10:614050a50c2f 165 else if (mode==4) {
DBerendsen 10:614050a50c2f 166 Led_red = 1;
DBerendsen 10:614050a50c2f 167 Led_green = 1;
DBerendsen 10:614050a50c2f 168 Led_blue = 0;
DBerendsen 10:614050a50c2f 169 }
DBerendsen 10:614050a50c2f 170 else if (mode==5) {
DBerendsen 10:614050a50c2f 171 Led_red = 0;
DBerendsen 10:614050a50c2f 172 Led_green = 0;
DBerendsen 10:614050a50c2f 173 Led_blue = 1;
DBerendsen 10:614050a50c2f 174 }*/
DBerendsen 10:614050a50c2f 175
peterknoben 5:b4abbd3c513c 176 }
peterknoben 5:b4abbd3c513c 177
peterknoben 5:b4abbd3c513c 178 // Control mode selection-------------------------------------------------------
peterknoben 5:b4abbd3c513c 179
peterknoben 5:b4abbd3c513c 180 //Determine the signalnumbers (i.e. speed) based on the strenght of the EMG signals
peterknoben 3:c768a37620c9 181 void signalnumber(){
peterknoben 4:e15fc329b88b 182 //Left
peterknoben 8:24f6744d47b2 183 Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);//*kLeft
peterknoben 8:24f6744d47b2 184 // mean_value_left = SignalLeft.getmeanLeft(n, Signal_filteredLeft);
peterknoben 8:24f6744d47b2 185 mean_value_left = SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())*kLeft));
peterknoben 8:24f6744d47b2 186 CaseLeft = SignalLeft.getnumberLeft(n, action, Signal_filteredLeft);
peterknoben 4:e15fc329b88b 187 //Right
peterknoben 8:24f6744d47b2 188 Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight
peterknoben 8:24f6744d47b2 189 // mean_value_right = SignalRight.getmeanRight(n, Signal_filteredRight);
peterknoben 8:24f6744d47b2 190 mean_value_right = SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())*kRight));
peterknoben 8:24f6744d47b2 191 CaseRight = SignalRight.getnumberRight(n, action, Signal_filteredRight);
peterknoben 5:b4abbd3c513c 192 //Mode
peterknoben 8:24f6744d47b2 193 /*
peterknoben 8:24f6744d47b2 194 Signal_filteredMode = fabs(FilterMode(emg4));//*kMode
peterknoben 8:24f6744d47b2 195 mean_value_mode = SignalMode.getmeanMode(n, Signal_filteredMode);
peterknoben 8:24f6744d47b2 196 CaseMode = SignalMode.getnumberMode(n, action, Signal_filteredMode);
peterknoben 8:24f6744d47b2 197 if(CaseMode >= 3){
peterknoben 5:b4abbd3c513c 198 milli ++;
peterknoben 5:b4abbd3c513c 199 if(milli>=150){
peterknoben 5:b4abbd3c513c 200 mode_selection();
peterknoben 5:b4abbd3c513c 201 milli=0;
peterknoben 5:b4abbd3c513c 202 }
peterknoben 5:b4abbd3c513c 203 }
peterknoben 5:b4abbd3c513c 204 else{
peterknoben 5:b4abbd3c513c 205 milli=0;
peterknoben 6:edd30e11f3c7 206 }*/
peterknoben 3:c768a37620c9 207 }
peterknoben 3:c768a37620c9 208
peterknoben 1:406519ff0f17 209 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 210 //-------------------------Kinematic Constants----------------------------------
peterknoben 1:406519ff0f17 211 //------------------------------------------------------------------------------
peterknoben 4:e15fc329b88b 212 const double RAD_PER_PULSE = 0.00074799825*2; //Number of radials per pulse from the encoder
peterknoben 4:e15fc329b88b 213 const double PI = 3.14159265358979323846; //Pi
DBerendsen 11:28b5ec12a4b9 214 const float max_rangex = 650.0, max_rangey = 450.0; //Max range on the y axis
peterknoben 8:24f6744d47b2 215 const float x_offset = 156.15, y_offset = -76.97; //Starting positions from the shoulder joint
DBerendsen 10:614050a50c2f 216 const float alpha_offset = -0.4114;
DBerendsen 10:614050a50c2f 217 const float beta_offset = 0.0486;
peterknoben 8:24f6744d47b2 218 const float L1 = 450.0, L2 = 490.0; //Length of the bodies
peterknoben 5:b4abbd3c513c 219
DBerendsen 0:5f4bc2d63814 220 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 221 //--------------------------------Motor1----------------------------------------
DBerendsen 0:5f4bc2d63814 222 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 223 FastPWM motor1(D5);
DBerendsen 0:5f4bc2d63814 224 DigitalOut motor1DirectionPin(D4);
DBerendsen 0:5f4bc2d63814 225 DigitalIn ENC2A(D12);
DBerendsen 0:5f4bc2d63814 226 DigitalIn ENC2B(D13);
DBerendsen 0:5f4bc2d63814 227 Encoder encoder1(D13,D12);
peterknoben 1:406519ff0f17 228 const float MOTOR1_KP = 40.0;
peterknoben 1:406519ff0f17 229 const float MOTOR1_KI = 0.0;
peterknoben 1:406519ff0f17 230 const float MOTOR1_KD = 15.0;
peterknoben 5:b4abbd3c513c 231 double M1_v1 = 0.0, M1_v2 = 0.0; //Calculation values
DBerendsen 0:5f4bc2d63814 232 const double motor1_gain = 2*PI;
peterknoben 1:406519ff0f17 233 const float M1_N = 0.5;
peterknoben 8:24f6744d47b2 234 float position_math_l =0, position_math_r=0;
DBerendsen 0:5f4bc2d63814 235
DBerendsen 10:614050a50c2f 236 float motor1_control(){
peterknoben 8:24f6744d47b2 237 float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math_l, position_math_r);
DBerendsen 0:5f4bc2d63814 238 float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
DBerendsen 0:5f4bc2d63814 239 float error_alpha = reference_alpha-position_alpha;
peterknoben 1:406519ff0f17 240 float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain;
DBerendsen 10:614050a50c2f 241
DBerendsen 10:614050a50c2f 242 //Determine Motor Magnitude
DBerendsen 10:614050a50c2f 243 if (fabs(magnitude1)>1) {
DBerendsen 10:614050a50c2f 244 motor1 = 1;
DBerendsen 10:614050a50c2f 245 }
DBerendsen 10:614050a50c2f 246 else {
DBerendsen 10:614050a50c2f 247 motor1 = fabs(magnitude1);
DBerendsen 10:614050a50c2f 248 }
DBerendsen 10:614050a50c2f 249
DBerendsen 10:614050a50c2f 250
peterknoben 1:406519ff0f17 251 // Determine Motor Direction
peterknoben 1:406519ff0f17 252 if (magnitude1 < 0){
DBerendsen 0:5f4bc2d63814 253 motor1DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 254 }
DBerendsen 0:5f4bc2d63814 255 else{
DBerendsen 0:5f4bc2d63814 256 motor1DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 257 }
DBerendsen 10:614050a50c2f 258 return magnitude1;
DBerendsen 0:5f4bc2d63814 259 }
DBerendsen 0:5f4bc2d63814 260
DBerendsen 0:5f4bc2d63814 261 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 262 //--------------------------------Motor2----------------------------------------
DBerendsen 0:5f4bc2d63814 263 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 264 FastPWM motor2(D6);
DBerendsen 0:5f4bc2d63814 265 DigitalOut motor2DirectionPin(D7);
DBerendsen 0:5f4bc2d63814 266 DigitalIn ENC1A(D10);
DBerendsen 0:5f4bc2d63814 267 DigitalIn ENC1B(D11);
DBerendsen 0:5f4bc2d63814 268 Encoder encoder2(D10,D11);
peterknoben 1:406519ff0f17 269 const float MOTOR2_KP = 60.0;
peterknoben 1:406519ff0f17 270 const float MOTOR2_KI = 0.0;
peterknoben 1:406519ff0f17 271 const float MOTOR2_KD = 15.0;
peterknoben 5:b4abbd3c513c 272 double M2_v1 = 0.0, M2_v2 = 0.0; //Calculation values
DBerendsen 0:5f4bc2d63814 273 const double motor2_gain = 2*PI;
peterknoben 1:406519ff0f17 274 const float M2_N = 0.5;
DBerendsen 0:5f4bc2d63814 275
DBerendsen 10:614050a50c2f 276 float motor2_control(){
peterknoben 8:24f6744d47b2 277 float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, position_math_l, position_math_r);
DBerendsen 0:5f4bc2d63814 278 float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
DBerendsen 0:5f4bc2d63814 279 float error_beta = reference_beta-position_beta;
peterknoben 1:406519ff0f17 280 float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain;
DBerendsen 10:614050a50c2f 281
DBerendsen 10:614050a50c2f 282 //Determine Motor Magnitude
DBerendsen 10:614050a50c2f 283 if (fabs(magnitude2)>1) {
DBerendsen 10:614050a50c2f 284 motor2 = 1;
DBerendsen 10:614050a50c2f 285 }
DBerendsen 10:614050a50c2f 286 else {
DBerendsen 10:614050a50c2f 287 motor2 = fabs(magnitude2);
DBerendsen 10:614050a50c2f 288 }
DBerendsen 10:614050a50c2f 289
DBerendsen 10:614050a50c2f 290
peterknoben 1:406519ff0f17 291 //Determine Motor Direction
DBerendsen 0:5f4bc2d63814 292 if (magnitude2 > 0){
DBerendsen 0:5f4bc2d63814 293 motor2DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 294 }
DBerendsen 0:5f4bc2d63814 295 else{
DBerendsen 0:5f4bc2d63814 296 motor2DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 297 }
DBerendsen 10:614050a50c2f 298 return magnitude2;
DBerendsen 0:5f4bc2d63814 299 }
peterknoben 5:b4abbd3c513c 300
peterknoben 8:24f6744d47b2 301 //------------------------------------------------------------------------------
peterknoben 8:24f6744d47b2 302 //----------------------------Motor controller----------------------------------
peterknoben 8:24f6744d47b2 303 //------------------------------------------------------------------------------
peterknoben 8:24f6744d47b2 304 int direction_l, direction_r;
DBerendsen 10:614050a50c2f 305 float magnitude1, magnitude2;
peterknoben 8:24f6744d47b2 306
peterknoben 5:b4abbd3c513c 307 void motor_control(){
DBerendsen 10:614050a50c2f 308 direction_l = MoveLeft.getdirectionLeft(mode); //x-direction
DBerendsen 10:614050a50c2f 309 direction_r = MoveRight.getdirectionRight(mode); //y-direction
DBerendsen 10:614050a50c2f 310 position_math_l= MoveLeft.getpositionLeft(CaseLeft, mode, max_rangex); //x-direction
DBerendsen 10:614050a50c2f 311 position_math_r= MoveRight.getpositionRight(CaseRight, mode, max_rangey); //y-direction
DBerendsen 10:614050a50c2f 312 magnitude1 = motor1_control();
DBerendsen 10:614050a50c2f 313 magnitude2 = motor2_control();
DBerendsen 10:614050a50c2f 314
DBerendsen 10:614050a50c2f 315
DBerendsen 10:614050a50c2f 316
DBerendsen 10:614050a50c2f 317
peterknoben 5:b4abbd3c513c 318 }
DBerendsen 11:28b5ec12a4b9 319 //------------------------------------------------------------------------------
peterknoben 8:24f6744d47b2 320 //-------------------------------HID Scope--------------------------------------
DBerendsen 0:5f4bc2d63814 321 //------------------------------------------------------------------------------
peterknoben 8:24f6744d47b2 322
peterknoben 8:24f6744d47b2 323 HIDScope scope(4);
peterknoben 8:24f6744d47b2 324 Ticker scopeTimer;
peterknoben 8:24f6744d47b2 325 Ticker controllerTimer;
peterknoben 8:24f6744d47b2 326
peterknoben 8:24f6744d47b2 327 void HID_Monitor(){
peterknoben 8:24f6744d47b2 328 scope.set(0, fabs(FilterLeft(emg0.read())));
peterknoben 8:24f6744d47b2 329 scope.set(1, fabs(FilterRight(emg2.read())));
peterknoben 8:24f6744d47b2 330 scope.set(2, SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read()))));
peterknoben 8:24f6744d47b2 331 scope.set(3, SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read()))));
peterknoben 8:24f6744d47b2 332 }
peterknoben 8:24f6744d47b2 333
peterknoben 8:24f6744d47b2 334 //Working
peterknoben 8:24f6744d47b2 335 //SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())))
DBerendsen 11:28b5ec12a4b9 336 //SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())))
peterknoben 8:24f6744d47b2 337
peterknoben 8:24f6744d47b2 338 //------------------------------------------------------------------------------
peterknoben 8:24f6744d47b2 339 //------------------------------Main--------------------------------------------
DBerendsen 0:5f4bc2d63814 340 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 341
DBerendsen 0:5f4bc2d63814 342 int main(){
DBerendsen 0:5f4bc2d63814 343 pc.baud(115200);
peterknoben 4:e15fc329b88b 344 setled();
peterknoben 6:edd30e11f3c7 345 BiQuad_filter_Left.add( &LP1L ).add( &HP2L ).add( &NO3L);
peterknoben 6:edd30e11f3c7 346 BiQuad_filter_Right.add( &LP1R ).add( &HP2R ).add( &NO3R);
peterknoben 6:edd30e11f3c7 347 BiQuad_filter_Mode.add( &LP1M ).add( &HP2M ).add( &NO3M);
peterknoben 4:e15fc329b88b 348 But1.rise(&maxcalibration);
peterknoben 6:edd30e11f3c7 349 Mode.rise(&mode_selection);
peterknoben 5:b4abbd3c513c 350 motor1.period(0.0001f); motor2.period(0.0001f);
peterknoben 8:24f6744d47b2 351 MyControllerTicker.attach(&motor_control, CONTROLLER_TS); //Determining motorposiitons
peterknoben 8:24f6744d47b2 352 MyTickerMean.attach(&signalnumber, MEAN_TS); //Detemining the signalnumbers
DBerendsen 11:28b5ec12a4b9 353 MyTickerServo.attach(&servo, 0.1f);
DBerendsen 11:28b5ec12a4b9 354
peterknoben 8:24f6744d47b2 355 //Scope
peterknoben 8:24f6744d47b2 356 scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
DBerendsen 11:28b5ec12a4b9 357 controllerTimer.attach_us(&HID_Monitor, 1e3);
peterknoben 8:24f6744d47b2 358
peterknoben 5:b4abbd3c513c 359 while(1) {
DBerendsen 10:614050a50c2f 360 //pc.printf(" direction %i , %i Signal numbers %i %i Position %f %f \r\n", direction_l, direction_r, CaseLeft, CaseRight, position_math_l, position_math_r);
DBerendsen 10:614050a50c2f 361 pc.printf("direction %i , %i Signal numbers %i %i Position %f %f magnitude1 =%f magnitude2=%f \r\n",direction_l, direction_r, CaseLeft, CaseRight, position_math_l, position_math_r, magnitude1, magnitude2);
peterknoben 5:b4abbd3c513c 362 wait(0.1f);
peterknoben 5:b4abbd3c513c 363 }
peterknoben 1:406519ff0f17 364 }
peterknoben 1:406519ff0f17 365
peterknoben 1:406519ff0f17 366