Peter Knoben / Mbed 2 deprecated kinematics_control_copyfds

sdfa

Dependencies:   AnglePosition2 Encoder FastPWM MODSERIAL Movement PIDController Servo SignalNumber2 biquadFilter mbed

Fork of kinematics_controlv4 by Peter Knoben

main.cpp@7:105e3ae0fb19, 2017-11-01 (annotated)

Committer:
peterknoben
Date:
Wed Nov 01 14:11:58 2017 +0000
Revision:
7:105e3ae0fb19
Parent:
6:edd30e11f3c7 
fa

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 5:b4abbd3c513c 17 Ticker MyControllerTicker; //Declare Ticker Motor 1
peterknoben 5:b4abbd3c513c 18 Ticker MyTickerMode; //Declare Ticker Motor 2
peterknoben 3:c768a37620c9 19 Ticker MyTickerMean; //Declare Ticker Signalprocessing
peterknoben 3:c768a37620c9 20
peterknoben 4:e15fc329b88b 21 InterruptIn But2(PTA4); //Declare button for min calibration
peterknoben 4:e15fc329b88b 22 InterruptIn But1(PTC6); //Declare button for max calibration
peterknoben 6:edd30e11f3c7 23 InterruptIn Mode(D7);
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 5:b4abbd3c513c 34 //AnalogIn emg1( A1 ); //Set Inputpin for EMG 1 signal Left
peterknoben 4:e15fc329b88b 35 AnalogIn emg2( A2 ); //Set Inputpin for EMG 2 signal Right
peterknoben 5:b4abbd3c513c 36 //AnalogIn emg3( A3 ); //Set Inputpin for EMG 3 signal Right
peterknoben 4:e15fc329b88b 37 AnalogIn emg4( A4 ); //Set Inputpin for EMG 4 signal Mode
peterknoben 5:b4abbd3c513c 38 //AnalogIn emg5( A5 ); //Set Inputpin for EMG 5 signal Mode
peterknoben 5:b4abbd3c513c 39 DigitalOut Up( D9 ); //Set digital in for mode selection
peterknoben 5:b4abbd3c513c 40 DigitalOut Down( D8 );
peterknoben 4:e15fc329b88b 41 DigitalOut Led_red(LED_RED);
peterknoben 4:e15fc329b88b 42 DigitalOut Led_green(LED_GREEN);
peterknoben 4:e15fc329b88b 43 DigitalOut Led_blue(LED_BLUE);
peterknoben 6:edd30e11f3c7 44 DigitalOut test(D2);
peterknoben 1:406519ff0f17 45
peterknoben 1:406519ff0f17 46 const float CONTROLLER_TS = 0.02; //Motor frequency
peterknoben 4:e15fc329b88b 47 const float MEAN_TS = 0.002; //Filter frequency
peterknoben 4:e15fc329b88b 48
peterknoben 3:c768a37620c9 49
peterknoben 3:c768a37620c9 50 //------------------------------------------------------------------------------
peterknoben 3:c768a37620c9 51 //-----------------------------EMG Signals--------------------------------------
peterknoben 3:c768a37620c9 52 //------------------------------------------------------------------------------
peterknoben 4:e15fc329b88b 53 // Filtering the signal and getting the usefull information out of it.
peterknoben 4:e15fc329b88b 54 const int n = 400; //Window size for the mean value, also adjust in signalnumber.cpp
peterknoben 3:c768a37620c9 55 const int action =50; //Number of same mean values to change the signalnumber
peterknoben 6:edd30e11f3c7 56 const int m = 400; //Number of samples for calibration
peterknoben 5:b4abbd3c513c 57 int CaseLeft, CaseRight, CaseMode; //Strength of the muscles
peterknoben 5:b4abbd3c513c 58 float emg_offsetLeft, emg_offsetmaxLeft; //Calibration offsets from zero to one for the left arm
peterknoben 5:b4abbd3c513c 59 float emg_offsetRight, emg_offsetmaxRight; //Calibration offsets from zero to one for the right arm
peterknoben 5:b4abbd3c513c 60 float emg_offsetMode, emg_offsetmaxMode;
peterknoben 5:b4abbd3c513c 61 float mean_value_left, mean_value_right, mean_value_mode; //Mean value of the filtered system
peterknoben 5:b4abbd3c513c 62 float kLeft, kRight, kMode; //Scaling factors
peterknoben 6:edd30e11f3c7 63 float Signal_filteredLeft, Signal_filteredRight, Signal_filteredMode;
peterknoben 3:c768a37620c9 64
peterknoben 4:e15fc329b88b 65 //BiQuad filter variables
peterknoben 6:edd30e11f3c7 66 BiQuad LP1L( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
peterknoben 6:edd30e11f3c7 67 BiQuad HP2L( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
peterknoben 6:edd30e11f3c7 68 BiQuad NO3L( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad
peterknoben 6:edd30e11f3c7 69 BiQuad LP1R( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
peterknoben 6:edd30e11f3c7 70 BiQuad HP2R( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
peterknoben 6:edd30e11f3c7 71 BiQuad NO3R( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad
peterknoben 6:edd30e11f3c7 72 BiQuad LP1M( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
peterknoben 6:edd30e11f3c7 73 BiQuad HP2M( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
peterknoben 6:edd30e11f3c7 74 BiQuad NO3M( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad
peterknoben 6:edd30e11f3c7 75 BiQuadChain BiQuad_filter_Left;
peterknoben 6:edd30e11f3c7 76 BiQuadChain BiQuad_filter_Right;
peterknoben 6:edd30e11f3c7 77 BiQuadChain BiQuad_filter_Mode;
peterknoben 3:c768a37620c9 78
peterknoben 7:105e3ae0fb19 79 float FilterLeft(float input){
peterknoben 7:105e3ae0fb19 80 float Signal_filtered= BiQuad_filter_Left.step(input);
peterknoben 7:105e3ae0fb19 81 return Signal_filtered;
peterknoben 7:105e3ae0fb19 82 }
peterknoben 7:105e3ae0fb19 83 float FilterRight(float input){
peterknoben 7:105e3ae0fb19 84 float Signal_filtered= BiQuad_filter_Right.step(input);
peterknoben 7:105e3ae0fb19 85 return Signal_filtered;
peterknoben 7:105e3ae0fb19 86 }
peterknoben 7:105e3ae0fb19 87 float FilterMode(float input){
peterknoben 7:105e3ae0fb19 88 float Signal_filtered= BiQuad_filter_Mode.step(input);
peterknoben 7:105e3ae0fb19 89 return Signal_filtered;
peterknoben 7:105e3ae0fb19 90 }
peterknoben 5:b4abbd3c513c 91
peterknoben 5:b4abbd3c513c 92 //Calibration-------------------------------------------------------------------
peterknoben 4:e15fc329b88b 93 void setled(){
peterknoben 5:b4abbd3c513c 94 Led_red=0; Led_green=1; Led_blue=1;
peterknoben 5:b4abbd3c513c 95 }
peterknoben 5:b4abbd3c513c 96 // Zero calibration
peterknoben 5:b4abbd3c513c 97 void mincalibration(){
peterknoben 5:b4abbd3c513c 98 pc.printf("start min calibration \r\n");
peterknoben 7:105e3ae0fb19 99 emg_offsetLeft = SignalLeft.calibrate(m,FilterLeft(emg0));
peterknoben 7:105e3ae0fb19 100 emg_offsetRight = SignalRight.calibrate(m,FilterRight(emg2));
peterknoben 7:105e3ae0fb19 101 emg_offsetMode = SignalMode.calibrate(m, FilterMode(emg4));
peterknoben 5:b4abbd3c513c 102 pc.printf("offsets: %f %f \r\n", emg_offsetLeft, emg_offsetRight);
peterknoben 5:b4abbd3c513c 103 Led_green=0; Led_red=0; //Set led to Yellow
peterknoben 5:b4abbd3c513c 104 }
peterknoben 5:b4abbd3c513c 105 // One calibration
peterknoben 5:b4abbd3c513c 106 void maxcalibration(){
peterknoben 5:b4abbd3c513c 107 pc.printf("start max calibration \r\n");
peterknoben 7:105e3ae0fb19 108 emg_offsetmaxLeft = SignalLeft.calibrate(m, FilterLeft(emg0))-emg_offsetLeft;
peterknoben 7:105e3ae0fb19 109 emg_offsetmaxRight = SignalRight.calibrate(m, FilterRight(emg2))-emg_offsetRight;
peterknoben 7:105e3ae0fb19 110 emg_offsetmaxMode = SignalMode.calibrate(m, FilterMode(emg4))-emg_offsetMode;
peterknoben 5:b4abbd3c513c 111 kLeft = 1/emg_offsetmaxLeft;
peterknoben 5:b4abbd3c513c 112 kRight = 1/emg_offsetmaxRight;
peterknoben 5:b4abbd3c513c 113 kMode = 1/emg_offsetmaxMode;
peterknoben 5:b4abbd3c513c 114 pc.printf("offsets: %f %f, scale %f %f \r\n", emg_offsetmaxLeft, emg_offsetmaxRight, kLeft, kRight);
peterknoben 5:b4abbd3c513c 115 Led_red=1; //Set led to Green
peterknoben 3:c768a37620c9 116 }
peterknoben 3:c768a37620c9 117
peterknoben 5:b4abbd3c513c 118 //Filtering the signals---------------------------------------------------------
peterknoben 4:e15fc329b88b 119 //Filter de EMG signals with a BiQuad filter
peterknoben 7:105e3ae0fb19 120 /*
peterknoben 6:edd30e11f3c7 121 float FilterLeft(float input, float offset){
peterknoben 6:edd30e11f3c7 122 float Signal=(input-offset); //((input0+input1)/2)
peterknoben 6:edd30e11f3c7 123 float Signal_filtered= BiQuad_filter_Left.step(Signal);
peterknoben 6:edd30e11f3c7 124 return Signal_filtered;
peterknoben 6:edd30e11f3c7 125 }
peterknoben 6:edd30e11f3c7 126 float FilterRight(float input, float offset){
peterknoben 5:b4abbd3c513c 127 float Signal=input-offset; //((input0+input1)/2)
peterknoben 6:edd30e11f3c7 128 float Signal_filtered= BiQuad_filter_Right.step(Signal);
peterknoben 6:edd30e11f3c7 129 return Signal_filtered;
peterknoben 6:edd30e11f3c7 130 }
peterknoben 6:edd30e11f3c7 131 float FilterMode(float input, float offset){
peterknoben 6:edd30e11f3c7 132 float Signal=input-offset; //((input0+input1)/2)
peterknoben 6:edd30e11f3c7 133 float Signal_filtered= BiQuad_filter_Mode.step(Signal);
peterknoben 3:c768a37620c9 134 return Signal_filtered;
peterknoben 3:c768a37620c9 135 }
peterknoben 7:105e3ae0fb19 136 */
peterknoben 4:e15fc329b88b 137
peterknoben 5:b4abbd3c513c 138 //------------------------------------------------------------------------------
peterknoben 5:b4abbd3c513c 139 //---------------------------------Servo----------------------------------------
peterknoben 5:b4abbd3c513c 140 //------------------------------------------------------------------------------
peterknoben 5:b4abbd3c513c 141 void servo(){
peterknoben 7:105e3ae0fb19 142 Signal_filteredLeft = fabs(FilterLeft(emg0)-emg_offsetLeft);//*kLeft
peterknoben 7:105e3ae0fb19 143 Signal_filteredRight = fabs(FilterRight(emg2)); //*kRight
peterknoben 7:105e3ae0fb19 144 CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft);
peterknoben 7:105e3ae0fb19 145 CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight);
peterknoben 5:b4abbd3c513c 146 if (CaseLeft>=3){
peterknoben 6:edd30e11f3c7 147 Up = 0;
peterknoben 6:edd30e11f3c7 148 Up = 1;
peterknoben 5:b4abbd3c513c 149 }
peterknoben 5:b4abbd3c513c 150 else if (CaseRight >=3){
peterknoben 6:edd30e11f3c7 151 Down = 0;
peterknoben 5:b4abbd3c513c 152 Down = 1;
peterknoben 5:b4abbd3c513c 153 }
peterknoben 5:b4abbd3c513c 154 }
peterknoben 5:b4abbd3c513c 155 int milli =0;
peterknoben 5:b4abbd3c513c 156
peterknoben 5:b4abbd3c513c 157 //------------------------------------------------------------------------------
peterknoben 5:b4abbd3c513c 158 //---------------------------Mode selection-------------------------------------
peterknoben 5:b4abbd3c513c 159 //------------------------------------------------------------------------------
peterknoben 5:b4abbd3c513c 160 int mode =0;
peterknoben 5:b4abbd3c513c 161
peterknoben 5:b4abbd3c513c 162 //Recieving mode selection from EMG mode signal
peterknoben 5:b4abbd3c513c 163 void mode_selection(){
peterknoben 5:b4abbd3c513c 164 if(mode ==6){
peterknoben 5:b4abbd3c513c 165 mode=1;
peterknoben 5:b4abbd3c513c 166 }
peterknoben 5:b4abbd3c513c 167 else{
peterknoben 5:b4abbd3c513c 168 mode++;
peterknoben 5:b4abbd3c513c 169 }
peterknoben 5:b4abbd3c513c 170 if (mode==3||mode==6){
peterknoben 5:b4abbd3c513c 171 servo();
peterknoben 5:b4abbd3c513c 172 }
peterknoben 5:b4abbd3c513c 173 pc.printf("\r\n mode = %i \r\n", mode);
peterknoben 5:b4abbd3c513c 174 }
peterknoben 5:b4abbd3c513c 175
peterknoben 5:b4abbd3c513c 176 // Control mode selection-------------------------------------------------------
peterknoben 5:b4abbd3c513c 177
peterknoben 6:edd30e11f3c7 178
peterknoben 6:edd30e11f3c7 179
peterknoben 6:edd30e11f3c7 180
peterknoben 6:edd30e11f3c7 181
peterknoben 6:edd30e11f3c7 182
peterknoben 6:edd30e11f3c7 183
peterknoben 5:b4abbd3c513c 184 //Determine the signalnumbers (i.e. speed) based on the strenght of the EMG signals
peterknoben 3:c768a37620c9 185 void signalnumber(){
peterknoben 4:e15fc329b88b 186 //Left
peterknoben 7:105e3ae0fb19 187 Signal_filteredLeft = fabs(FilterLeft(emg0)-emg_offsetLeft);//*kLeft
peterknoben 7:105e3ae0fb19 188 mean_value_left = SignalLeft.getmean(n, Signal_filteredLeft);
peterknoben 7:105e3ae0fb19 189 CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft);
peterknoben 4:e15fc329b88b 190 //Right
peterknoben 7:105e3ae0fb19 191 Signal_filteredRight = fabs(FilterRight(emg2)*kRight);
peterknoben 7:105e3ae0fb19 192 mean_value_right = SignalRight.getmean(n, Signal_filteredRight);
peterknoben 7:105e3ae0fb19 193 CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight);
peterknoben 5:b4abbd3c513c 194 //Mode
peterknoben 7:105e3ae0fb19 195 Signal_filteredMode = fabs(FilterMode(emg4)*kMode);
peterknoben 7:105e3ae0fb19 196 mean_value_mode = SignalMode.getmean(n, Signal_filteredMode);
peterknoben 7:105e3ae0fb19 197 CaseMode = SignalMode.getnumber(n, action, Signal_filteredMode);
peterknoben 6:edd30e11f3c7 198 /*if(CaseMode >= 3){
peterknoben 5:b4abbd3c513c 199 milli ++;
peterknoben 5:b4abbd3c513c 200 if(milli>=150){
peterknoben 5:b4abbd3c513c 201 mode_selection();
peterknoben 5:b4abbd3c513c 202 milli=0;
peterknoben 5:b4abbd3c513c 203 }
peterknoben 5:b4abbd3c513c 204 }
peterknoben 5:b4abbd3c513c 205 else{
peterknoben 5:b4abbd3c513c 206 milli=0;
peterknoben 6:edd30e11f3c7 207 }*/
peterknoben 3:c768a37620c9 208 }
peterknoben 3:c768a37620c9 209
peterknoben 5:b4abbd3c513c 210
peterknoben 5:b4abbd3c513c 211
peterknoben 6:edd30e11f3c7 212
peterknoben 6:edd30e11f3c7 213
peterknoben 6:edd30e11f3c7 214
peterknoben 6:edd30e11f3c7 215
peterknoben 6:edd30e11f3c7 216
peterknoben 1:406519ff0f17 217 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 218 //-------------------------Kinematic Constants----------------------------------
peterknoben 1:406519ff0f17 219 //------------------------------------------------------------------------------
peterknoben 4:e15fc329b88b 220 const double RAD_PER_PULSE = 0.00074799825*2; //Number of radials per pulse from the encoder
peterknoben 4:e15fc329b88b 221 const double PI = 3.14159265358979323846; //Pi
peterknoben 4:e15fc329b88b 222 const float max_rangex = 500.0; //Max range on the x axis
peterknoben 4:e15fc329b88b 223 const float max_rangey = 300.0; //Max range on the y axis
peterknoben 4:e15fc329b88b 224 const float x_offset = 156.15; //Start x position from the shoulder joint
peterknoben 4:e15fc329b88b 225 const float y_offset = -76.97; //Start y position from the shoulder joint
peterknoben 4:e15fc329b88b 226 const float alpha_offset = -(21.52/180)*PI; //Begin angle Alpha at zero zero
peterknoben 4:e15fc329b88b 227 const float beta_offset = 0.0; //Begin angle Beta at zero zero
peterknoben 4:e15fc329b88b 228 const float L1 = 450.0; //Length of the first body
peterknoben 4:e15fc329b88b 229 const float L2 = 490.0; //Length of the second body
DBerendsen 0:5f4bc2d63814 230
DBerendsen 0:5f4bc2d63814 231
peterknoben 5:b4abbd3c513c 232
DBerendsen 0:5f4bc2d63814 233 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 234 //--------------------------------Motor1----------------------------------------
DBerendsen 0:5f4bc2d63814 235 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 236 FastPWM motor1(D5);
DBerendsen 0:5f4bc2d63814 237 DigitalOut motor1DirectionPin(D4);
DBerendsen 0:5f4bc2d63814 238 DigitalIn ENC2A(D12);
DBerendsen 0:5f4bc2d63814 239 DigitalIn ENC2B(D13);
DBerendsen 0:5f4bc2d63814 240 Encoder encoder1(D13,D12);
peterknoben 1:406519ff0f17 241 const float MOTOR1_KP = 40.0;
peterknoben 1:406519ff0f17 242 const float MOTOR1_KI = 0.0;
peterknoben 1:406519ff0f17 243 const float MOTOR1_KD = 15.0;
peterknoben 5:b4abbd3c513c 244 double M1_v1 = 0.0, M1_v2 = 0.0; //Calculation values
DBerendsen 0:5f4bc2d63814 245 const double motor1_gain = 2*PI;
peterknoben 1:406519ff0f17 246 const float M1_N = 0.5;
peterknoben 6:edd30e11f3c7 247 float position_math[2]={};
DBerendsen 0:5f4bc2d63814 248
DBerendsen 0:5f4bc2d63814 249 void motor1_control(){
peterknoben 4:e15fc329b88b 250 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 251 float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
DBerendsen 0:5f4bc2d63814 252 float error_alpha = reference_alpha-position_alpha;
peterknoben 1:406519ff0f17 253 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 254 motor1 = fabs(magnitude1);
peterknoben 1:406519ff0f17 255 // Determine Motor Direction
peterknoben 1:406519ff0f17 256 if (magnitude1 < 0){
DBerendsen 0:5f4bc2d63814 257 motor1DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 258 }
DBerendsen 0:5f4bc2d63814 259 else{
DBerendsen 0:5f4bc2d63814 260 motor1DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 261 }
DBerendsen 0:5f4bc2d63814 262 }
DBerendsen 0:5f4bc2d63814 263
DBerendsen 0:5f4bc2d63814 264 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 265 //--------------------------------Motor2----------------------------------------
DBerendsen 0:5f4bc2d63814 266 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 267 FastPWM motor2(D6);
DBerendsen 0:5f4bc2d63814 268 DigitalOut motor2DirectionPin(D7);
DBerendsen 0:5f4bc2d63814 269 DigitalIn ENC1A(D10);
DBerendsen 0:5f4bc2d63814 270 DigitalIn ENC1B(D11);
DBerendsen 0:5f4bc2d63814 271 Encoder encoder2(D10,D11);
peterknoben 1:406519ff0f17 272 const float MOTOR2_KP = 60.0;
peterknoben 1:406519ff0f17 273 const float MOTOR2_KI = 0.0;
peterknoben 1:406519ff0f17 274 const float MOTOR2_KD = 15.0;
peterknoben 5:b4abbd3c513c 275 double M2_v1 = 0.0, M2_v2 = 0.0; //Calculation values
DBerendsen 0:5f4bc2d63814 276 const double motor2_gain = 2*PI;
peterknoben 1:406519ff0f17 277 const float M2_N = 0.5;
DBerendsen 0:5f4bc2d63814 278
DBerendsen 0:5f4bc2d63814 279 void motor2_control(){
peterknoben 4:e15fc329b88b 280 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 281 float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
DBerendsen 0:5f4bc2d63814 282 float error_beta = reference_beta-position_beta;
peterknoben 1:406519ff0f17 283 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 284 motor2 = fabs(magnitude2);
peterknoben 1:406519ff0f17 285 //Determine Motor Direction
DBerendsen 0:5f4bc2d63814 286 if (magnitude2 > 0){
DBerendsen 0:5f4bc2d63814 287 motor2DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 288 }
DBerendsen 0:5f4bc2d63814 289 else{
DBerendsen 0:5f4bc2d63814 290 motor2DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 291 }
DBerendsen 0:5f4bc2d63814 292 }
peterknoben 5:b4abbd3c513c 293
peterknoben 5:b4abbd3c513c 294 void motor_control(){
peterknoben 5:b4abbd3c513c 295 position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex);
peterknoben 5:b4abbd3c513c 296 position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey);
peterknoben 5:b4abbd3c513c 297 motor1_control();
peterknoben 5:b4abbd3c513c 298 motor2_control();
peterknoben 5:b4abbd3c513c 299 }
DBerendsen 0:5f4bc2d63814 300 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 301 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 302 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 303
DBerendsen 0:5f4bc2d63814 304 int main(){
DBerendsen 0:5f4bc2d63814 305 pc.baud(115200);
peterknoben 6:edd30e11f3c7 306 test=0;
peterknoben 4:e15fc329b88b 307 setled();
peterknoben 6:edd30e11f3c7 308 BiQuad_filter_Left.add( &LP1L ).add( &HP2L ).add( &NO3L);
peterknoben 6:edd30e11f3c7 309 BiQuad_filter_Right.add( &LP1R ).add( &HP2R ).add( &NO3R);
peterknoben 6:edd30e11f3c7 310 BiQuad_filter_Mode.add( &LP1M ).add( &HP2M ).add( &NO3M);
peterknoben 4:e15fc329b88b 311 But2.rise(&mincalibration);
peterknoben 4:e15fc329b88b 312 But1.rise(&maxcalibration);
peterknoben 6:edd30e11f3c7 313 Mode.rise(&mode_selection);
peterknoben 5:b4abbd3c513c 314 motor1.period(0.0001f); motor2.period(0.0001f);
peterknoben 5:b4abbd3c513c 315 MyControllerTicker.attach(&motor_control, CONTROLLER_TS);
peterknoben 3:c768a37620c9 316 MyTickerMean.attach(&signalnumber, MEAN_TS);
peterknoben 5:b4abbd3c513c 317 // MyTickerMode.attach(&signalmode, MEAN_TS);
peterknoben 5:b4abbd3c513c 318 // MyTickerMean.attach(&signalnumberright, MEAN_TS);
peterknoben 5:b4abbd3c513c 319 // MyTickerMean.attach(&signalmode,MEAN_TS);
peterknoben 4:e15fc329b88b 320
peterknoben 5:b4abbd3c513c 321 while(1) {
peterknoben 6:edd30e11f3c7 322 // pc.printf("Mean %f %f %f \r\n", mean_value_left, mean_value_right, mean_value_mode);
peterknoben 5:b4abbd3c513c 323 // pc.printf("Case %i %i %i, mode = %i \r\n", CaseLeft, CaseRight, CaseMode, mode);
peterknoben 6:edd30e11f3c7 324 pc.printf("mean %f , case = %i \r\n", mean_value_left, CaseLeft);
peterknoben 5:b4abbd3c513c 325 wait(0.1f);
peterknoben 5:b4abbd3c513c 326 }
peterknoben 1:406519ff0f17 327 }
peterknoben 1:406519ff0f17 328
peterknoben 1:406519ff0f17 329