![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Mbed bordje 1 -af
Dependencies: Encoder HIDScope MODSERIAL Matrix MatrixMath biquad-master mbed
Fork of dsjklafjaslkjdfalkjfdaslkasdjklajadsflkjdasflkjdasflkadsflkasd by
main.cpp
- Committer:
- RoyvZ
- Date:
- 2017-11-03
- Revision:
- 6:643c44fb044a
- Parent:
- 5:3562c205d001
File content as of revision 6:643c44fb044a:
//initialisation #include "mbed.h" #include "encoder.h" #include "HIDScope.h" #include "MODSERIAL.h" #include "math.h" #include "BiQuad.h" #include "Matrix.h" #include "MatrixMath.h" #include <stdlib.h> #include <iostream> #include <iomanip> #include <complex> #include <vector> Ticker MainTicker; //Define objects EMG AnalogIn emg0( A0 ); AnalogIn emg1( A1 ); DigitalIn buttonCal(D11); DigitalIn changeX(D2); DigitalIn changeY(D3); InterruptIn turning(D10); DigitalIn buttonMove(PTC6); Ticker sample_timer; Ticker calX_timer; Ticker calY_timer; //HIDScope scope( 4 ); DigitalOut led(LED1); DigitalOut led2(LED_GREEN); //Define variables EMG const int size = 100; vector<double> SX(size,0); vector<double> SY(size,0); double meanSum = 0; double maxsignal = 0; //Filters BiQuadChain Notch; BiQuadChain Notch50; BiQuadChain bqcLP; BiQuadChain bqc; //Notch filter chain for 100 Hz BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 ); BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 ); BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 ); //Notch filter chain for 50 Hz BiQuad bq1( 9.50972e-01, -1.53921e+00, 9.51003e-01, -1.57886e+00, 9.50957e-01 ); BiQuad bq2( 1.00000e+00, -1.61851e+00, 9.99979e-01, -1.57185e+00, 9.74451e-01 ); BiQuad bq3( 1.00000e+00, -1.61855e+00, 9.99988e-01, -1.62358e+00, 9.75921e-01 ); //2 Hz High Pass filter BiQuad Highpass1(9.82385e-01, -1.96477e+00, 9.82385e-01, -1.96446e+00, 9.65081e-01); //80 Hz Low Pass filter BiQuad bqLP1( 5.78904e-02, 5.78898e-02, 0.00000e+00, -2.90527e-01, 0.00000e+00 ); BiQuad bqLP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -7.53537e-01, 4.06308e-01 ); //Define objects MotorControl DigitalOut gpo(D0); DigitalOut UpperMotorDir(D4); PwmOut UpperMotorVel(D5); DigitalOut LowerMotorDir(D6); PwmOut LowerMotorVel(D7); AnalogIn potMeterIn(A1); InterruptIn ButMotorDir(D3); Encoder encoder1(D13,D12); Encoder encoder2(D9,D8); Ticker m1_Ticker; Ticker setpoint_Ticker; //Define variables MotorControl float M_PI = 3.14159265359; float M1_KP = 1.0f; float M1_KI = 0.1f; const float M1_TS = 0.002; const float q_RAD_PER_PULSE_m2 = 0.36f*M_PI/1300.0f; //2*M_PI/4320 voor als het nu geen rechte lijn meer is8 const float q_RAD_PER_PULSE_m1 = 0.5f*M_PI/3300.0f; float m1_err_int = 0; float m2_err_int = 0; int motorD1 = 0; float motor1 = 0; int motorD2 = 0; float motor2 = 0; float position1 = 0; float position2 = 0; float emgX = 0; float emgY = 0; float L0 = 0.123; float L1 = 0.37; float L2 = 0.41; float q1 = 0.0f*M_PI; float q2 = 0.0f*M_PI; float Periode = 0.001; //1000 is het aantal Hz float ref_pos1; float ref_pos2; float Qset1 = 0; float Qset2 = 0; int UpperMotorPos; int UpperMotorPos_prev; int LowerMotorPos; int LowerMotorPos_prev; int NPC = 0; float NoTurnies = 1; //Define matrices MotorControl Matrix JAPPAPP(2,2); Matrix qdot(2,1); Matrix Vdes(2,1); //Define object Servo Motors DigitalOut Ledr(LED_RED); DigitalOut Ledg(LED_GREEN); DigitalOut Ledb(LED_BLUE); PwmOut LeftServoVel(D11); PwmOut RightServoVel(D10); DigitalIn ServoButton(D2); Ticker ServoTick; //Other variables float Cal_Strength = 0.6f; //Connection with TeraTerm MODSERIAL pc(USBTX, USBRX); //Creating of the states enum states{calib1, calib2, calibEMG, UpDown, LeftRight, Vertical, MotionEndEffector, GrabbingPlacing, Off, Idle, No_State}; //Root Mean Square value calculator for useful envelope, using vectors double findRMS(vector<double> array) { int i; double sumsquared = 0.000; //define variable that resets to 0 for every call of the function double RMS; for (i = 0; i < size; i++) //for loop to calculate the total sum of the squared values { sumsquared = sumsquared + array.at(i)*array.at(i); } RMS = sqrt((double(1)/size)*(sumsquared)); //root mean square calculation return RMS; } //Function to show the EMG signals in HIDScope void sample() { /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ //Standard emg value without filters, offset removed with the - value //scope.set(0, emg0.read()-0.4542f); //Use S vector to save values coming from EMG signal SX.erase(SX.begin()); //remove first value double emgXvalue = bqc.step(emg0.read()-0.4542f); SX.push_back(emgXvalue); //add extra value to begin of vector SY.erase(SY.begin()); //remove first value double emgYvalue = bqc.step(emg1.read()-0.4542f); SY.push_back(emgYvalue); //add extra value to begin of vector //Calculate Root Mean Square value using findRMS function as defined above double emgX = findRMS(SX); double emgY = findRMS(SY); //Display results //scope.set(1, emgX); //scope.set(2, emgY); //Send all scopes to HIDScope //scope.send(); /* To indicate that the function is working, the LED is toggled */ led = !led; } //Calibration function for the EMG signals void calibrationX() { //function to determine maximal EMG input in order to allow motorcontrol to be activated if (buttonCal.read() == false){ //activated when button is pressed led2 = !led2; for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds SX.erase(SX.begin()); double emgXCalvalue = bqc.step(emg0.read()-0.4542f); SX.push_back(emgXCalvalue); double signalfinal = findRMS(SX); //calculate RMS values from EMG signals if (signalfinal >= maxsignal){ //Check if current signal is higher than current maximal signal maxsignal = signalfinal; //keep changing the maximal signal } } led2 = 1; //Turn LED off when calibration is finished } } void calibrationY() { //function to determine maximal EMG input in order to allow motorcontrol to be activated if (buttonCal.read() == false){ //activated when button is pressed led2 = !led2; for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds SY.erase(SY.begin()); double emgYCalvalue = bqc.step(emg1.read()-0.4542f); SY.push_back(emgYCalvalue); double signalfinal = findRMS(SY); //calculate RMS values from EMG signals if (signalfinal >= maxsignal){ //Check if current signal is higher than current maximal signal maxsignal = signalfinal; //keep changing the maximal signal } } led2 = 1; //Turn LED off when calibration is finished } } void NegPosChanger(){ if(NPC == 0){ NoTurnies = 0; NPC++; } else { NoTurnies = 1; NPC= 0; } } void qSetpointSet() { // Fill Matrix with data. q1 = encoder1.getPosition()*q_RAD_PER_PULSE_m1; //Calibreren nog toevoegen q2 = 0.5f*M_PI - q1 + encoder2.getPosition()*q_RAD_PER_PULSE_m2; //Calibreren mist nog JAPPAPP(1,1) = (cos(q1)*cos(q2) - sin(q1)*sin(q2))/(L1*sin(q2)*cos(q1)*cos(q1) + L1*sin(q2)*sin(q1)*sin(q1)); JAPPAPP(1,2) = (cos(q1)*sin(q2) + cos(q2)*sin(q1))/(L1*sin(q2)*cos(q1)*cos(q1) + L1*sin(q2)*sin(q1)*sin(q1)); JAPPAPP(2,1) = -(L1*cos(q1) + L2*cos(q1)*cos(q2) - L2*sin(q1)*sin(q2))/(L1*L2*sin(q2)*cos(q1)*cos(q1) + L1*L2*sin(q2)*sin(q1)*sin(q1)); JAPPAPP(2,2) = -(L1*sin(q1) + L2*cos(q1)*sin(q2) + L2*cos(q2)*sin(q1))/(L1*L2*sin(q2)*cos(q1)*cos(q1) + L1*L2*sin(q2)*sin(q1)*sin(q1)); // Fill Matrix with data. int Xchanger = (changeX == 1) ? -1 : 1; int Ychanger = (changeY == 1) ? -1 : 1; Vdes(1,1) = (emgX-0.1f);//*Xchanger*NoTurnies; Vdes(2,1) = (emgY-0.1f);//*Ychanger*NoTurnies; int turnChanger = (turning == 1) ? -1 : 1; qdot = JAPPAPP*Vdes; Qset1 = Qset1 + qdot(1,1)*Periode; //Vdes(1,1) Qset2 = Qset2 + qdot(2,1)*Periode; //Vdes(2,1) if (Qset1 <= 0.43f*M_PI && Qset1 >= 0.01f){ Qset1 = Qset1; } else if (Qset1 < 0.01f){ Qset1 = 0.01f; } else if (Qset1 > 0.43f*M_PI){ Qset1 = 0.43f*M_PI; } if (Qset1 + Qset2 < 0.5f*M_PI){ Qset2 = 0.5f*M_PI - Qset1; } if (Qset2 <= 0.5f*M_PI && Qset2 >= 0.1f){ Qset2 = Qset2; } else if (Qset2 < .1f){ Qset2 = 0.1f; } else if (Qset1+Qset2 > M_PI){ Qset2 = M_PI - Qset1; } } float PI( float e, const float Kp, const float Ki, float Ts, float &e_int ){ // Reusable PI controller return fabs(Kp * e); } void PositionControl() { //position1 = RAD_PER_PULSE * encoder1.getPosition(); //position2 = RAD_PER_PULSE * encoder2.getPosition(); ref_pos1 = Qset1 - q1; //if (ref_pos1 < 0.1) { ref_pos1 = 0; } ref_pos2 = Qset2 - q2; // Don’t use magic numbers! //if (ref_pos2 < 0.1) { ref_pos2 = 0; } motor1 = PI(ref_pos1, M1_KP, M1_KI, M1_TS, m1_err_int ); motor2 = PI(ref_pos2, M1_KP, M1_KI, M1_TS, m2_err_int ); if (ref_pos1 <= 0) { motorD1=1; } else{ motorD1 = 0; } if (ref_pos2 <= 0) { motorD2=0; } else{ motorD2=1; } UpperMotorDir.write(motorD1); UpperMotorVel.write(motor1); LowerMotorDir.write(motorD2); LowerMotorVel.write(motor2); } /** Servo motor control function **/ //Servo initialisation enum servostates{Close, Open}; //Set default state to open, as mentioned in paragraph ... of the report servostates CurrentState = Open; int i = 0; int j = 0; //Simply switch the current state when button is pressed void Change() { if(CurrentState == Close){ CurrentState = Open; } else{ CurrentState = Close; } } //State machine for the servo motors, to determine the state void ProcessStateMachine(void) { /** State machine for the servo motors, motor response dependent on the current state and the amount of milliseconds of activation. The entire function must be sampled at 2000 Hz, since it counts half milliseconds. This is required since the servos respond to half millisecond actuation. For further information on the functioning of the servo motors; see paragraph ... in report 'Design of a Jenga playing robot' by group 2. **/ switch (CurrentState) { case Close: if(j <= 35){ RightServoVel.write(0); LeftServoVel.write(0); j++; } else if (j == 36){ RightServoVel.write(1); LeftServoVel.write(0); j++; } else if (j == 37){ RightServoVel.write(1); LeftServoVel.write(0); j++; } else if (j == 38){ RightServoVel.write(1); LeftServoVel.write(0); j++; } else if (j == 39){ RightServoVel.write(1); LeftServoVel.write(1); j = 0; } break; case Open: if(j <= 35){ RightServoVel.write(0); LeftServoVel.write(0); j++; } else if (j == 36){ RightServoVel.write(0); LeftServoVel.write(0); j++; } else if (j == 37){ RightServoVel.write(0); LeftServoVel.write(1); j++; } else if (j == 38){ RightServoVel.write(1); LeftServoVel.write(1); j++; } else if (j == 39){ RightServoVel.write(1); LeftServoVel.write(1); j = 0; } break; } } //StateMachine for the moving of the end effector void ProcessStateMachineMain(void) { states CurrentState1 = MotionEndEffector; switch (CurrentState1) { case MotionEndEffector: setpoint_Ticker.attach(&qSetpointSet, Periode); m1_Ticker.attach( &PositionControl, M1_TS ); break; case calibEMG: //Function to calibrate the filtered EMG signals to the user, in order to control the motors double maxsignal = 0; //empty variable to check the maximal EMG output for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 5 seconds double signalfinal = fabs(bqc.step(emg0.read())); //get values from the filtered EMG signal if (signalfinal >= maxsignal){ //check whether the received signal is higher or lower than the current highest signal value maxsignal = signalfinal; //keep changing the maximal signal } } break; case calib1: UpperMotorVel = Cal_Strength; UpperMotorDir = true; //depends on definitions LowerMotorVel = 0.0f; UpperMotorPos = encoder1.getPosition(); if (abs(UpperMotorPos - UpperMotorPos_prev) < 100){ if (Cal_Strength < 0.7f) { Cal_Strength += 0.05f; } else{ encoder1.setPosition(0); //Afhankelijk van RKI uitzoeken of deze echt naar nul moet of naar 0.5pi CurrentState1 = calib2; Cal_Strength = 0.6f; } UpperMotorPos_prev = UpperMotorPos; } break; case calib2: LowerMotorVel = Cal_Strength; LowerMotorDir = true; // Check definitions UpperMotorVel = 0.3f; UpperMotorDir = true; LowerMotorPos = encoder2.getPosition(); if (abs(LowerMotorPos - LowerMotorPos_prev) < 10){ if (Cal_Strength < 1.0f) { Cal_Strength += 0.05f; } else{ encoder2.setPosition(0); CurrentState1 = Idle; //M1_controller = true; //turn on the position controller here UpperMotorVel = 0.0f; LowerMotorVel = 0.0f; } LowerMotorPos_prev = LowerMotorPos; } break; case Idle: emgX = 0; emgY = 0; m1_Ticker.attach( &PositionControl, M1_TS ); setpoint_Ticker.attach(&qSetpointSet, Periode); break; case Off: //Robot is off, also return to default state so you have a reference position when it starts up again break; default: //Robot is in idle mode, not doing anything (motors still powered to hold current position) (Posible when waiting for your turn) } } //Main function for the servo motors, to change open or closed state int main() { //allow for the 2000 Hz sampling of the Servo state machine function //MainTicker.attach(ProcessStateMachineMain, 0.0005); //ServoTick.attach(ProcessStateMachine, 0.0005); /** EMG signals **/ //Constructing the notch filter chain bqc.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ).add( &bqLP1 ).add( &bqLP2 ); //Sample every 0.002 seconds, or 500 Hz sample_timer.attach(&sample, 0.002); //Ticker to check if motor is being calibrated, also at 500 Hz calX_timer.attach(&calibrationX, 0.002); calY_timer.attach(&calibrationY, 0.002); //Turn LED off, turned on when calibrating led2 = 1; turning.rise(&NegPosChanger); //MODSERIAL connection pc.baud(115200); setpoint_Ticker.attach(&qSetpointSet, Periode); m1_Ticker.attach( &PositionControl, M1_TS ); while (true) { /* if (buttonCal.read() == false){ //when calibrate button is pressed states CurrentState1 = calibEMG; } if (buttonMove.read() == false){ states CurrentState1 = MotionEndEffector; } */ /* if (!ServoButton) { Change(); wait(0.3); } */ /*Control end-effector wait(0.1); float v_ref = GetReferenceVelocity(); setMotor(v_ref); pc.printf("%f \r\n", FeedForwardControl(v_ref)); UpperMotorDir.write(motorDirection); UpperMotorVel.write(RightServoVel); //PWM Speed Control */ } }