Casper Kroon / Mbed 2 deprecated State_machine2

State machine

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of State_machine by Kilian Buitenhuis

Constants.h@7:af586102d80c, 2018-11-07 (annotated)

Committer:
CasperK
Date:
Wed Nov 07 13:51:55 2018 +0000
Revision:
7:af586102d80c
Parent:
6:344e075c1221 
Final code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wabbitdrawing 5:e96d03bd557c 1 #ifndef CONSTANTS
Wabbitdrawing 5:e96d03bd557c 2 #define CONSTANTS
Wabbitdrawing 5:e96d03bd557c 3
Wabbitdrawing 5:e96d03bd557c 4 PwmOut pwmpin1(D6);
Wabbitdrawing 5:e96d03bd557c 5 PwmOut pwmpin2(D5);
Wabbitdrawing 5:e96d03bd557c 6 AnalogIn potmeter1(A5);
Wabbitdrawing 5:e96d03bd557c 7 AnalogIn potmeter2(A4);
Wabbitdrawing 5:e96d03bd557c 8 DigitalIn button1(D2);
Wabbitdrawing 5:e96d03bd557c 9 DigitalIn button2(D3);
CasperK 7:af586102d80c 10 DigitalOut directionpin2(D4);
CasperK 7:af586102d80c 11 DigitalOut directionpin1(D7);
Wabbitdrawing 5:e96d03bd557c 12 QEI motor1(D13,D12,NC, 32);
Wabbitdrawing 5:e96d03bd557c 13 QEI motor2(D11,D10,NC, 32);
Wabbitdrawing 5:e96d03bd557c 14
Wabbitdrawing 5:e96d03bd557c 15 //Define objects
Wabbitdrawing 5:e96d03bd557c 16 AnalogIn emg0( A0 ); // EMG at A0
Wabbitdrawing 5:e96d03bd557c 17 BiQuad emg0bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1
Wabbitdrawing 5:e96d03bd557c 18 BiQuad emg0bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
Wabbitdrawing 5:e96d03bd557c 19 BiQuad emg0bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
Wabbitdrawing 5:e96d03bd557c 20 BiQuadChain emg0bqc; // merged chain of three filters
Wabbitdrawing 5:e96d03bd557c 21
Wabbitdrawing 5:e96d03bd557c 22 AnalogIn emg1( A1 ); // EMG at A1
Wabbitdrawing 5:e96d03bd557c 23 BiQuad emg1bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1
Wabbitdrawing 5:e96d03bd557c 24 BiQuad emg1bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
Wabbitdrawing 5:e96d03bd557c 25 BiQuad emg1bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
Wabbitdrawing 5:e96d03bd557c 26 BiQuadChain emg1bqc; // merged chain of three filters
Wabbitdrawing 5:e96d03bd557c 27
Wabbitdrawing 5:e96d03bd557c 28 AnalogIn emg2( A2 ); // EMG at A2
Wabbitdrawing 5:e96d03bd557c 29 BiQuad emg2bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1
Wabbitdrawing 5:e96d03bd557c 30 BiQuad emg2bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
Wabbitdrawing 5:e96d03bd557c 31 BiQuad emg2bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
Wabbitdrawing 5:e96d03bd557c 32 BiQuadChain emg2bqc; // merged chain of three filters
Wabbitdrawing 5:e96d03bd557c 33
CasperK 6:344e075c1221 34 InterruptIn kill_switch(SW2); //position has to be changed
Wabbitdrawing 5:e96d03bd557c 35 DigitalIn next_switch(SW3); //name and position should be replaced
Wabbitdrawing 5:e96d03bd557c 36
Wabbitdrawing 5:e96d03bd557c 37 enum position_states{PositionCalibration, EmgCalibration, Movement, KILL};
Wabbitdrawing 5:e96d03bd557c 38 enum emg_states{EMG0, EMG1, EMG2};
Wabbitdrawing 5:e96d03bd557c 39 position_states CurrentState;
Wabbitdrawing 5:e96d03bd557c 40 emg_states CalibrationState;
Wabbitdrawing 5:e96d03bd557c 41
Wabbitdrawing 5:e96d03bd557c 42 Ticker sample_timer;
Wabbitdrawing 5:e96d03bd557c 43 Ticker MotorsTicker;
CasperK 7:af586102d80c 44 Ticker stateticker;
Wabbitdrawing 5:e96d03bd557c 45 Timer timer;
Wabbitdrawing 5:e96d03bd557c 46
Wabbitdrawing 5:e96d03bd557c 47 //for testing purposes
Wabbitdrawing 5:e96d03bd557c 48 DigitalOut ledred(LED_RED);
Wabbitdrawing 5:e96d03bd557c 49 DigitalOut ledgreen(LED_GREEN);
Wabbitdrawing 5:e96d03bd557c 50 DigitalOut ledblue(LED_BLUE);
Wabbitdrawing 5:e96d03bd557c 51 //MODSERIAL pc(USBTX, USBRX);
CasperK 7:af586102d80c 52 //HIDScope scope(6);
Wabbitdrawing 5:e96d03bd557c 53
Wabbitdrawing 5:e96d03bd557c 54 bool emg0Bool = 0; // I don't know if these NEED to be global, but when I tried to put them in they wouldn't work...
Wabbitdrawing 5:e96d03bd557c 55 int emg0Ignore = 0;
Wabbitdrawing 5:e96d03bd557c 56 bool emg1Bool = 0;
Wabbitdrawing 5:e96d03bd557c 57 int emg1Ignore = 0;
Wabbitdrawing 5:e96d03bd557c 58 bool emg2Bool = 0;
Wabbitdrawing 5:e96d03bd557c 59 int emg2Ignore = 0;
Wabbitdrawing 5:e96d03bd557c 60 float Calibration0;
Wabbitdrawing 5:e96d03bd557c 61 float Calibration1;
Wabbitdrawing 5:e96d03bd557c 62 float Calibration2;
Wabbitdrawing 5:e96d03bd557c 63
Wabbitdrawing 5:e96d03bd557c 64 double input = 0; // raw input
Wabbitdrawing 5:e96d03bd557c 65 double filtHigh = 0; // filtered after highpass
Wabbitdrawing 5:e96d03bd557c 66 double filtlow = 0; // filtered after lowpass
Wabbitdrawing 5:e96d03bd557c 67 double filtNotch = 0; // filtered after notch
Wabbitdrawing 5:e96d03bd557c 68 double emg0filteredAbs;
Wabbitdrawing 5:e96d03bd557c 69
Wabbitdrawing 5:e96d03bd557c 70 float threshold0;
Wabbitdrawing 5:e96d03bd557c 71 float threshold1;
Wabbitdrawing 5:e96d03bd557c 72 float threshold2;
Wabbitdrawing 5:e96d03bd557c 73
Wabbitdrawing 5:e96d03bd557c 74 volatile float pwm_value1 = 0.0;
Wabbitdrawing 5:e96d03bd557c 75 volatile float pwm_value2 = 0.0;
Wabbitdrawing 5:e96d03bd557c 76
CasperK 7:af586102d80c 77 const float C1 = 3.0; //motor 1 gear ratio
CasperK 7:af586102d80c 78 const float C2 = 0.013; //motor 2 gear ratio/radius of the circular gear in m
CasperK 7:af586102d80c 79 const float length = 0.300; //length in m (placeholder)
CasperK 7:af586102d80c 80 const float Timestep = 0.01;
CasperK 7:af586102d80c 81
CasperK 7:af586102d80c 82 volatile bool x_direction = true;
CasperK 7:af586102d80c 83
CasperK 7:af586102d80c 84 volatile float x_position = length;
CasperK 7:af586102d80c 85 volatile float y_position = 0.0;
CasperK 7:af586102d80c 86 volatile float old_x_position;
CasperK 7:af586102d80c 87 volatile float old_y_position;
CasperK 7:af586102d80c 88 volatile float x_correction;
CasperK 7:af586102d80c 89 volatile float old_x_correction;
CasperK 7:af586102d80c 90 volatile float old_motor1_angle;
CasperK 7:af586102d80c 91 volatile float old_motor2_angle;
CasperK 7:af586102d80c 92 volatile float motor1_angle = 0.0; //sawtooth gear motor
CasperK 7:af586102d80c 93 volatile float motor2_angle = 0.0; //rotational gear motor
CasperK 7:af586102d80c 94 volatile float direction;
CasperK 7:af586102d80c 95
CasperK 7:af586102d80c 96 //values of PID controller
CasperK 7:af586102d80c 97 const float Kp = 5;
CasperK 7:af586102d80c 98 const float Ki = 0.02;
CasperK 7:af586102d80c 99 const float Kd = 0.05;
CasperK 7:af586102d80c 100 volatile float Output1 = 0 ; //Starting value
CasperK 7:af586102d80c 101 volatile float Output2 = 0 ; //Starting value
CasperK 7:af586102d80c 102 volatile float P1 = 0; //encoder value
CasperK 7:af586102d80c 103 volatile float P2 = 0;
CasperK 7:af586102d80c 104 volatile float e1 = 0 ; //Starting value
CasperK 7:af586102d80c 105 volatile float e2 = 0 ; //Starting value
CasperK 7:af586102d80c 106 volatile float e3 = 0;
CasperK 7:af586102d80c 107 volatile float f1 = 0 ; //Starting value
CasperK 7:af586102d80c 108 volatile float f2 = 0 ; //Starting value
CasperK 7:af586102d80c 109 volatile float f3 = 0;
CasperK 7:af586102d80c 110 volatile float df3 = 0;
CasperK 7:af586102d80c 111 volatile float if3 = 0;
CasperK 7:af586102d80c 112 volatile float de3 = 0;
CasperK 7:af586102d80c 113 volatile float ie3 = 0;
CasperK 7:af586102d80c 114
CasperK 7:af586102d80c 115 volatile float Output_Last1; // Remember previous position
CasperK 7:af586102d80c 116 volatile float Output_Last2; // Remember previous position
CasperK 7:af586102d80c 117 volatile float Y1 = 0; // Value that is outputted to motor control
CasperK 7:af586102d80c 118 volatile float Y2 = 0; // Value that is outputted to motor control
CasperK 7:af586102d80c 119 volatile float P_Last = 0; // Starting position
CasperK 7:af586102d80c 120
Wabbitdrawing 5:e96d03bd557c 121 #endif