Eindelijk!!!!!
Dependencies: AnglePosition2 Encoder FastPWM HIDScope MODSERIAL Movement PIDController Servo SignalNumber biquadFilter mbed
Fork of Robot_control by
main.cpp
- Committer:
- peterknoben
- Date:
- 2017-10-30
- Revision:
- 3:c768a37620c9
- Parent:
- 1:406519ff0f17
- Child:
- 4:e15fc329b88b
File content as of revision 3:c768a37620c9:
#include "MODSERIAL.h" #include "AnglePosition.h" #include "PIDControl.h" #include "BiQuad.h" #include "signalnumber.h" #include "mbed.h" #include "encoder.h" #include "Servo.h" #include "FastPWM.h" //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ MODSERIAL pc(USBTX, USBRX); //Establish connection Ticker MyControllerTicker1; //Declare Ticker Motor 1 Ticker MyControllerTicker2; //Declare Ticker Motor 2 Ticker MySampleTicker; //Declare Ticker HIDscope Ticker MyTickerMean; //Declare Ticker Signalprocessing InterruptIn But2(PTC6); //Declare button for calibration AnglePosition Angle; //Declare Angle calculater PIDControl PID; //Declare PID Controller SignalNumber Signal; //Declare Signal determiner AnalogIn potmeter1(A5); AnalogIn potmeter5(A3); //Set Inputpin AnalogIn potmeter2(A4); //Set Inputpin AnalogIn emg0( A0 ); //Set Inputpin AnalogIn emg1( A1 ); //Set Inputpin const float CONTROLLER_TS = 0.02; //Motor frequency const float MEAN_TS = 0.1; //Mean value frequency const float SAMPLE_TS = 0.02; //BiQuad frequency //------------------------------------------------------------------------------ //-----------------------------EMG Signals-------------------------------------- //------------------------------------------------------------------------------ const int n = 10; //Window size for the mean value, also adjust in signalnumber.cpp const int action =50; //Number of same mean values to change the signalnumber const int m = 10; //Number of samples for calibration int mode; float emg_offset; float meanx; //BiQuad filter BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad BiQuad HP2( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad BiQuadChain BiQuad_filter; // Calibration function void calibration(){ emg_offset = Signal.calibrate(m, potmeter5); pc.printf("offset = %f \r\n", emg_offset); } /* //Sample function void sample() { float Signal=((emg0+emg1)/2)-emg_offset; float Signal_filtered= BiQuad_filter.step(Signal); /* scope scope.set(0, emg0.read() ); scope.set(1, emg1.read() ); scope.set(2, Signal_filtered); scope.send(); led = !led; / } */ float Filter(float input0, float input1, float offset){ float Signal=((input0+input1)/2)-offset; float Signal_filtered= BiQuad_filter.step(Signal); return Signal_filtered; } void signalnumber(){ float Signal_filtered = Filter(emg0, emg1, emg_offset); meanx = Signal.getmean(n, Signal_filtered); mode = Signal.getnumber(n, action, Signal_filtered); pc.printf("pot = %d after filtering = %f mean = %f Signalnumber = %i \r\n" , potmeter5, meanx, mode); } //------------------------------------------------------------------------------ //-------------------------Kinematic Constants---------------------------------- //------------------------------------------------------------------------------ const double RAD_PER_PULSE = 0.00074799825*2; const double PI = 3.14159265358979323846; const float max_rangex = 500.0; const float max_rangey = 300.0; const float x_offset = 156.15; const float y_offset = -76.97; const float alpha_offset = -(21.52/180)*PI; const float beta_offset = 0.0; const float L1 = 450.0; const float L2 = 490.0; //------------------------------------------------------------------------------ //--------------------------------Servo----------------------------------------- //------------------------------------------------------------------------------ // This will be programmed on a different Mbed Servo MyServo(D9); //Declare button InterruptIn But1(D8); //Declare used button int k=0; //Position flag void servo_control (){ if (k==0){ MyServo = 0; k=1; } else{ MyServo = 2; k=0; } } //------------------------------------------------------------------------------ //--------------------------------Motor1---------------------------------------- //------------------------------------------------------------------------------ FastPWM motor1(D5); DigitalOut motor1DirectionPin(D4); DigitalIn ENC2A(D12); DigitalIn ENC2B(D13); Encoder encoder1(D13,D12); const float MOTOR1_KP = 40.0; const float MOTOR1_KI = 0.0; const float MOTOR1_KD = 15.0; double M1_v1 = 0.0; double M1_v2 = 0.0; const double motor1_gain = 2*PI; const float M1_N = 0.5; void motor1_control(){ float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, potmeter1, potmeter2); float position_alpha = RAD_PER_PULSE * encoder1.getPosition(); float error_alpha = reference_alpha-position_alpha; float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain; motor1 = fabs(magnitude1); // pc.printf("err_a = %f alpha = %f mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1); // pc.printf("\r\n"); // Determine Motor Direction if (magnitude1 < 0){ motor1DirectionPin = 1; } else{ motor1DirectionPin = 0; } } //------------------------------------------------------------------------------ //--------------------------------Motor2---------------------------------------- //------------------------------------------------------------------------------ FastPWM motor2(D6); DigitalOut motor2DirectionPin(D7); DigitalIn ENC1A(D10); DigitalIn ENC1B(D11); Encoder encoder2(D10,D11); const float MOTOR2_KP = 60.0; const float MOTOR2_KI = 0.0; const float MOTOR2_KD = 15.0; double m2_err_int = 0; const double motor2_gain = 2*PI; const float M2_N = 0.5; double M2_v1 = 0.0; double M2_v2 = 0.0; void motor2_control(){ float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, potmeter1, potmeter2); float position_beta = RAD_PER_PULSE * -encoder2.getPosition(); float error_beta = reference_beta-position_beta; float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain; motor2 = fabs(magnitude2); // pc.printf("err2 = %f beta = %f mag2 = %f\r\n", error_beta, reference_beta, magnitude2); // pc.printf("\r\n"); //Determine Motor Direction if (magnitude2 > 0){ motor2DirectionPin = 1; } else{ motor2DirectionPin = 0; } } //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ int main(){ pc.baud(115200); BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3); motor1.period(0.0001f); motor2.period(0.0001f); MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS); MyTickerMean.attach(&signalnumber, MEAN_TS); // MySampleTicker.attach(&sample, SAMPLE_TS); But1.rise(&servo_control); But2.rise(&calibration); while(1) {} }