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@4:e15fc329b88b, 2017-10-31 (annotated)

Committer:
peterknoben
Date:
Tue Oct 31 14:35:52 2017 +0000
Revision:
4:e15fc329b88b
Parent:
3:c768a37620c9
Child:
5:b4abbd3c513c
sdfasdf

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"
DBerendsen 0:5f4bc2d63814 11
peterknoben 4:e15fc329b88b 12 //This code is for Mbed 2
DBerendsen 0:5f4bc2d63814 13 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 14 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 15 //------------------------------------------------------------------------------
peterknoben 3:c768a37620c9 16 MODSERIAL pc(USBTX, USBRX); //Establish connection
peterknoben 1:406519ff0f17 17 Ticker MyControllerTicker1; //Declare Ticker Motor 1
peterknoben 1:406519ff0f17 18 Ticker MyControllerTicker2; //Declare Ticker Motor 2
peterknoben 4:e15fc329b88b 19 Ticker MySampleTicker; //Declare Ticker HIDscope
peterknoben 3:c768a37620c9 20 Ticker MyTickerMean; //Declare Ticker Signalprocessing
peterknoben 3:c768a37620c9 21
peterknoben 4:e15fc329b88b 22 InterruptIn But2(PTA4); //Declare button for min calibration
peterknoben 4:e15fc329b88b 23 InterruptIn But1(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 4:e15fc329b88b 29 Movement MoveLeft; //Declare Movement determiner
peterknoben 4:e15fc329b88b 30 Movement MoveRight;
peterknoben 3:c768a37620c9 31
peterknoben 4:e15fc329b88b 32 AnalogIn emg0( A0 ); //Set Inputpin for EMG 0 signal Left
peterknoben 4:e15fc329b88b 33 AnalogIn emg1( A1 ); //Set Inputpin for EMG 1 signal Left
peterknoben 4:e15fc329b88b 34 AnalogIn emg2( A2 ); //Set Inputpin for EMG 2 signal Right
peterknoben 4:e15fc329b88b 35 AnalogIn emg3( A3 ); //Set Inputpin for EMG 3 signal Right
peterknoben 4:e15fc329b88b 36 AnalogIn emg4( A4 ); //Set Inputpin for EMG 4 signal Mode
peterknoben 4:e15fc329b88b 37 AnalogIn emg5( A5 ); //Set Inputpin for EMG 5 signal Mode
peterknoben 4:e15fc329b88b 38 DigitalOut M( D9 ); //Set digital in for mode selection
peterknoben 4:e15fc329b88b 39 DigitalOut Led_red(LED_RED);
peterknoben 4:e15fc329b88b 40 DigitalOut Led_green(LED_GREEN);
peterknoben 4:e15fc329b88b 41 DigitalOut Led_blue(LED_BLUE);
peterknoben 1:406519ff0f17 42
peterknoben 1:406519ff0f17 43 const float CONTROLLER_TS = 0.02; //Motor frequency
peterknoben 4:e15fc329b88b 44 const float MEAN_TS = 0.002; //Filter frequency
peterknoben 4:e15fc329b88b 45
peterknoben 4:e15fc329b88b 46 //Testing methods
peterknoben 4:e15fc329b88b 47 /*
peterknoben 4:e15fc329b88b 48 AnalogIn potmeter1(A5);
peterknoben 4:e15fc329b88b 49 AnalogIn potmeter5(A3); //Set Inputpin for x axis
peterknoben 4:e15fc329b88b 50 AnalogIn potmeter2(A4); //Set Inputpin for y axis
peterknoben 4:e15fc329b88b 51 */
peterknoben 4:e15fc329b88b 52
peterknoben 4:e15fc329b88b 53 //------------------------------------------------------------------------------
peterknoben 4:e15fc329b88b 54 //---------------------------Mode selection-------------------------------------
peterknoben 4:e15fc329b88b 55 //------------------------------------------------------------------------------
peterknoben 4:e15fc329b88b 56 // From the other Mbed there will be send a signal to determine in which mode the system is in
peterknoben 4:e15fc329b88b 57 int mode =0;
peterknoben 4:e15fc329b88b 58
peterknoben 4:e15fc329b88b 59 //Recieving mode selection from Mbed 1
peterknoben 4:e15fc329b88b 60 void mode_selection(){
peterknoben 4:e15fc329b88b 61 if(mode ==6){
peterknoben 4:e15fc329b88b 62 mode=1;
peterknoben 4:e15fc329b88b 63 }
peterknoben 4:e15fc329b88b 64 else{
peterknoben 4:e15fc329b88b 65 mode++;
peterknoben 4:e15fc329b88b 66 }
peterknoben 4:e15fc329b88b 67 pc.printf("mode = %i\r\n", mode);
peterknoben 4:e15fc329b88b 68 }
peterknoben 4:e15fc329b88b 69
peterknoben 3:c768a37620c9 70
peterknoben 3:c768a37620c9 71
peterknoben 3:c768a37620c9 72 //------------------------------------------------------------------------------
peterknoben 3:c768a37620c9 73 //-----------------------------EMG Signals--------------------------------------
peterknoben 3:c768a37620c9 74 //------------------------------------------------------------------------------
peterknoben 4:e15fc329b88b 75 // Filtering the signal and getting the usefull information out of it.
peterknoben 4:e15fc329b88b 76 const int n = 400; //Window size for the mean value, also adjust in signalnumber.cpp
peterknoben 3:c768a37620c9 77 const int action =50; //Number of same mean values to change the signalnumber
peterknoben 4:e15fc329b88b 78 const int m = 300; //Number of samples for calibration
peterknoben 4:e15fc329b88b 79 int CaseLeft; //Strength of the muscles Left
peterknoben 4:e15fc329b88b 80 int CaseRight; //Strength of the muscles Right
peterknoben 4:e15fc329b88b 81 float emg_offsetLeft; //Calibtarion value to get zero
peterknoben 4:e15fc329b88b 82 float emg_offsetmaxLeft; //Calibration value to scale to 1
peterknoben 4:e15fc329b88b 83 float emg_offsetRight; //Calibtarion value to get zero
peterknoben 4:e15fc329b88b 84 float emg_offsetmaxRight; //Calibration value to scale to 1
peterknoben 4:e15fc329b88b 85 float meanxL; //Temporary variable for mean value
peterknoben 4:e15fc329b88b 86 float meanxR;
peterknoben 4:e15fc329b88b 87 float kLeft; //Scaling factor mean value
peterknoben 4:e15fc329b88b 88 float kRight; //Scaling factor mean value
peterknoben 3:c768a37620c9 89
peterknoben 4:e15fc329b88b 90 //BiQuad filter variables
peterknoben 3:c768a37620c9 91 BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
peterknoben 3:c768a37620c9 92 BiQuad HP2( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
peterknoben 3:c768a37620c9 93 BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad
peterknoben 3:c768a37620c9 94 BiQuadChain BiQuad_filter;
peterknoben 3:c768a37620c9 95
peterknoben 4:e15fc329b88b 96 void setled(){
peterknoben 4:e15fc329b88b 97 Led_red=0;
peterknoben 4:e15fc329b88b 98 Led_green=1;
peterknoben 4:e15fc329b88b 99 Led_blue=1;
peterknoben 3:c768a37620c9 100 }
peterknoben 3:c768a37620c9 101
peterknoben 4:e15fc329b88b 102 // Calibration function
peterknoben 4:e15fc329b88b 103 void mincalibration(){
peterknoben 4:e15fc329b88b 104 pc.printf("start cali \r\n");
peterknoben 4:e15fc329b88b 105 emg_offsetLeft = SignalLeft.calibrate(m,((emg0+emg1)/2));
peterknoben 4:e15fc329b88b 106 emg_offsetRight = SignalRight.calibrate(m,((emg2+emg3)/2));
peterknoben 4:e15fc329b88b 107 // pc.printf("calibrated, offset = %f \r\n", emg_offset);
peterknoben 4:e15fc329b88b 108 Led_green=0;
peterknoben 4:e15fc329b88b 109 Led_red=0;
peterknoben 3:c768a37620c9 110 }
peterknoben 3:c768a37620c9 111
peterknoben 4:e15fc329b88b 112 void maxcalibration(){
peterknoben 4:e15fc329b88b 113 pc.printf("start cali max\r\n");
peterknoben 4:e15fc329b88b 114 emg_offsetmaxLeft = SignalLeft.calibrate(m,((emg0+emg1)/2))-emg_offsetLeft;
peterknoben 4:e15fc329b88b 115 emg_offsetmaxRight = SignalRight.calibrate(m,((emg2+emg3)/2))-emg_offsetRight;
peterknoben 4:e15fc329b88b 116 kLeft = 1/emg_offsetmaxLeft;
peterknoben 4:e15fc329b88b 117 kRight = 1/emg_offsetmaxRight;
peterknoben 4:e15fc329b88b 118 // pc.printf("calibrated, offset = %f scale = %f \r\n",emg_offsetmax, k);
peterknoben 4:e15fc329b88b 119 Led_red=1;
peterknoben 4:e15fc329b88b 120 }
peterknoben 4:e15fc329b88b 121
peterknoben 4:e15fc329b88b 122 //Filter de EMG signals with a BiQuad filter
peterknoben 3:c768a37620c9 123 float Filter(float input0, float input1, float offset){
peterknoben 4:e15fc329b88b 124 float Signal=input0-offset; //((input0+input1)/2)
peterknoben 3:c768a37620c9 125 float Signal_filtered= BiQuad_filter.step(Signal);
peterknoben 3:c768a37620c9 126 return Signal_filtered;
peterknoben 3:c768a37620c9 127 }
peterknoben 3:c768a37620c9 128
peterknoben 4:e15fc329b88b 129
peterknoben 4:e15fc329b88b 130 //Determine the signalnumbers (i.e. speed) based on the EMG signals
peterknoben 3:c768a37620c9 131 void signalnumber(){
peterknoben 4:e15fc329b88b 132 //Left
peterknoben 4:e15fc329b88b 133 float Signal_filteredLeft = fabs(Filter(emg0, emg1, emg_offsetLeft));
peterknoben 4:e15fc329b88b 134 meanxL = SignalLeft.getmean(n, Signal_filteredLeft)*kLeft; //Testing variable
peterknoben 4:e15fc329b88b 135 CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft, kLeft);
peterknoben 4:e15fc329b88b 136 pc.printf("m %f C %i \r\n",meanxL, CaseLeft); //Testing print
peterknoben 4:e15fc329b88b 137 //Right
peterknoben 4:e15fc329b88b 138 float Signal_filteredRight = fabs(Filter(emg2, emg3, emg_offsetRight));
peterknoben 4:e15fc329b88b 139 meanxR = SignalRight.getmean(n, Signal_filteredRight)*kRight; //Testing variable
peterknoben 4:e15fc329b88b 140 CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight, kRight);
peterknoben 4:e15fc329b88b 141 pc.printf("m %f C %i \r\n",meanxR, CaseRight); //Testing print
peterknoben 3:c768a37620c9 142 }
peterknoben 3:c768a37620c9 143
peterknoben 1:406519ff0f17 144 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 145 //-------------------------Kinematic Constants----------------------------------
peterknoben 1:406519ff0f17 146 //------------------------------------------------------------------------------
peterknoben 4:e15fc329b88b 147 const double RAD_PER_PULSE = 0.00074799825*2; //Number of radials per pulse from the encoder
peterknoben 4:e15fc329b88b 148 const double PI = 3.14159265358979323846; //Pi
peterknoben 4:e15fc329b88b 149 const float max_rangex = 500.0; //Max range on the x axis
peterknoben 4:e15fc329b88b 150 const float max_rangey = 300.0; //Max range on the y axis
peterknoben 4:e15fc329b88b 151 const float x_offset = 156.15; //Start x position from the shoulder joint
peterknoben 4:e15fc329b88b 152 const float y_offset = -76.97; //Start y position from the shoulder joint
peterknoben 4:e15fc329b88b 153 const float alpha_offset = -(21.52/180)*PI; //Begin angle Alpha at zero zero
peterknoben 4:e15fc329b88b 154 const float beta_offset = 0.0; //Begin angle Beta at zero zero
peterknoben 4:e15fc329b88b 155 const float L1 = 450.0; //Length of the first body
peterknoben 4:e15fc329b88b 156 const float L2 = 490.0; //Length of the second body
DBerendsen 0:5f4bc2d63814 157
DBerendsen 0:5f4bc2d63814 158
DBerendsen 0:5f4bc2d63814 159 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 160 //--------------------------------Motor1----------------------------------------
DBerendsen 0:5f4bc2d63814 161 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 162 FastPWM motor1(D5);
DBerendsen 0:5f4bc2d63814 163 DigitalOut motor1DirectionPin(D4);
DBerendsen 0:5f4bc2d63814 164 DigitalIn ENC2A(D12);
DBerendsen 0:5f4bc2d63814 165 DigitalIn ENC2B(D13);
DBerendsen 0:5f4bc2d63814 166 Encoder encoder1(D13,D12);
peterknoben 1:406519ff0f17 167 const float MOTOR1_KP = 40.0;
peterknoben 1:406519ff0f17 168 const float MOTOR1_KI = 0.0;
peterknoben 1:406519ff0f17 169 const float MOTOR1_KD = 15.0;
peterknoben 1:406519ff0f17 170 double M1_v1 = 0.0;
peterknoben 1:406519ff0f17 171 double M1_v2 = 0.0;
DBerendsen 0:5f4bc2d63814 172 const double motor1_gain = 2*PI;
peterknoben 1:406519ff0f17 173 const float M1_N = 0.5;
DBerendsen 0:5f4bc2d63814 174
DBerendsen 0:5f4bc2d63814 175
DBerendsen 0:5f4bc2d63814 176 void motor1_control(){
peterknoben 4:e15fc329b88b 177 float *position_math;
peterknoben 4:e15fc329b88b 178 position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex);
peterknoben 4:e15fc329b88b 179 position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey);
peterknoben 4:e15fc329b88b 180 float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math[0], position_math[1]);
DBerendsen 0:5f4bc2d63814 181 float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
DBerendsen 0:5f4bc2d63814 182 float error_alpha = reference_alpha-position_alpha;
peterknoben 1:406519ff0f17 183 float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain;
DBerendsen 0:5f4bc2d63814 184 motor1 = fabs(magnitude1);
peterknoben 1:406519ff0f17 185 // Determine Motor Direction
peterknoben 1:406519ff0f17 186 if (magnitude1 < 0){
DBerendsen 0:5f4bc2d63814 187 motor1DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 188 }
DBerendsen 0:5f4bc2d63814 189 else{
DBerendsen 0:5f4bc2d63814 190 motor1DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 191 }
DBerendsen 0:5f4bc2d63814 192 }
DBerendsen 0:5f4bc2d63814 193
DBerendsen 0:5f4bc2d63814 194 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 195 //--------------------------------Motor2----------------------------------------
DBerendsen 0:5f4bc2d63814 196 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 197 FastPWM motor2(D6);
DBerendsen 0:5f4bc2d63814 198 DigitalOut motor2DirectionPin(D7);
DBerendsen 0:5f4bc2d63814 199 DigitalIn ENC1A(D10);
DBerendsen 0:5f4bc2d63814 200 DigitalIn ENC1B(D11);
DBerendsen 0:5f4bc2d63814 201 Encoder encoder2(D10,D11);
peterknoben 1:406519ff0f17 202 const float MOTOR2_KP = 60.0;
peterknoben 1:406519ff0f17 203 const float MOTOR2_KI = 0.0;
peterknoben 1:406519ff0f17 204 const float MOTOR2_KD = 15.0;
DBerendsen 0:5f4bc2d63814 205 double m2_err_int = 0;
DBerendsen 0:5f4bc2d63814 206 const double motor2_gain = 2*PI;
peterknoben 1:406519ff0f17 207 const float M2_N = 0.5;
peterknoben 1:406519ff0f17 208 double M2_v1 = 0.0;
peterknoben 1:406519ff0f17 209 double M2_v2 = 0.0;
DBerendsen 0:5f4bc2d63814 210
DBerendsen 0:5f4bc2d63814 211
DBerendsen 0:5f4bc2d63814 212 void motor2_control(){
peterknoben 4:e15fc329b88b 213 float *position_math;
peterknoben 4:e15fc329b88b 214 position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex);
peterknoben 4:e15fc329b88b 215 position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey);
peterknoben 4:e15fc329b88b 216 float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, position_math[0], position_math[1]);
DBerendsen 0:5f4bc2d63814 217 float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
DBerendsen 0:5f4bc2d63814 218 float error_beta = reference_beta-position_beta;
peterknoben 1:406519ff0f17 219 float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain;
DBerendsen 0:5f4bc2d63814 220 motor2 = fabs(magnitude2);
peterknoben 1:406519ff0f17 221 //Determine Motor Direction
DBerendsen 0:5f4bc2d63814 222 if (magnitude2 > 0){
DBerendsen 0:5f4bc2d63814 223 motor2DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 224 }
DBerendsen 0:5f4bc2d63814 225 else{
DBerendsen 0:5f4bc2d63814 226 motor2DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 227 }
DBerendsen 0:5f4bc2d63814 228 }
DBerendsen 0:5f4bc2d63814 229 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 230 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 231 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 232
DBerendsen 0:5f4bc2d63814 233 int main(){
DBerendsen 0:5f4bc2d63814 234 pc.baud(115200);
peterknoben 4:e15fc329b88b 235 setled();
peterknoben 3:c768a37620c9 236 BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3);
peterknoben 4:e15fc329b88b 237 But2.rise(&mincalibration);
peterknoben 4:e15fc329b88b 238 But1.rise(&maxcalibration);
peterknoben 4:e15fc329b88b 239 // M.rise(&mode_selection);
peterknoben 1:406519ff0f17 240 motor1.period(0.0001f);
peterknoben 1:406519ff0f17 241 motor2.period(0.0001f);
DBerendsen 0:5f4bc2d63814 242 MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS);
DBerendsen 0:5f4bc2d63814 243 MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
peterknoben 3:c768a37620c9 244 MyTickerMean.attach(&signalnumber, MEAN_TS);
peterknoben 4:e15fc329b88b 245
DBerendsen 0:5f4bc2d63814 246 while(1) {}
peterknoben 1:406519ff0f17 247 }
peterknoben 1:406519ff0f17 248
peterknoben 1:406519ff0f17 249