The code without functions and variables filled in
Dependencies: HIDScope Servo mbed QEI biquadFilter
THE.cpp
- Committer:
- s1574396
- Date:
- 2018-11-02
- Revision:
- 22:be961d1830bb
- Parent:
- 21:bf09f236c9fa
- Child:
- 23:cc8b95d7eb57
File content as of revision 22:be961d1830bb:
#include "mbed.h" // Use revision 119!! #include "HIDScope.h" // For displaying data, select MBED - HID device, restart for every new code #include "QEI.h" // For reading the encoder of the motors #include <ctime> // for the timer during the process (if needed) #include "Servo.h" // For controlling the servo #include "BiQuad.h" #define SERIAL_BAUD 115200 // In- en outputs // ----------------------------------------------------------------------------- // EMG AnalogIn emg0_in( A0 ); // x_direction AnalogIn emg1_in( A1 ); // y_direction AnalogIn emg2_in( A2 ); // changing directions // Motor related DigitalOut dirpin_1(D4); // direction of motor 1 (translation) PwmOut pwmpin_1(D5); // PWM pin of motor 1 DigitalOut dirpin_2(D7); // direction of motor 2 (rotation) PwmOut pwmpin_2(D6); // PWM pin of motor 2 // Extra stuff DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed DigitalIn button_emergency(D8); // button for emergency mode, on bioshield DigitalIn button_wait(SW3); // button for wait mode, on mbed DigitalIn button_demo(D9); // button for demo mode, on bioshield DigitalIn led_red(LED_RED); // red led DigitalIn led_green(LED_GREEN); // green led DigitalIn led_blue(LED_BLUE); // blue led Servo myservo(D3); // Define the servo to control (in penholder), up to start with // Other stuff // ----------------------------------------------------------------------------- // Define stuff like tickers etc Ticker process_tick; Ticker emergency; HIDScope scope(6); // Number of channels in HIDScope QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); // Define the type of encoding: X4 encoding(default is X2) QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); Serial pc(USBTX,USBRX); Timer t; // For timing the time in each state (https://os.mbed.com/handbook/Timer) // Variables // ----------------------------------------------------------------------------- // Define here all variables needed throughout the whole code volatile double time_overall; volatile double time_in_state; volatile double motor_velocity = 0; volatile double EMG = 0; volatile double errors = 0; volatile int counts1_prev = 0; volatile int counts2_prev = 0; volatile int counts1; volatile int counts2; // EMG related // Constants EMG filter const double m1 = 0.5000; const double m2 = -0.8090; const double n0 = 0.5000; const double n1 = -0.8090; const double n2 = 0.0; const double a1 = 0.9565; const double a2 = -1.9131; const double b0 = 0.9565; const double b1 = -1.9112; const double b2 = 0.9150; const double c1 = 0.0675; const double c2 = 0.1349; const double d0 = 0.0675; const double d1 = -1.1430; const double d2 = 0.4128; // Variables EMG double emg0; double emg1; double emg2; double notch0; double notch1; double notch2; double high0; double high1; double high2; double absolute0; double absolute1; double absolute2; double low0; double low1; double low2; // BiQuad values BiQuadChain notch; BiQuad N1( m1, m2, n0, n1, n2); BiQuad N2( m1, m2, n0, n1, n2); BiQuad N3( m1, m2, n0, n1, n2); BiQuadChain highpass; BiQuad H1( a1, a2, b0, b1, b2); BiQuad H2( a1, a2, b0, b1, b2); BiQuad H3( a1, a2, b0, b1, b2); BiQuadChain lowpass; BiQuad L1( c1, c2, d0, d1, d2); BiQuad L2( c1, c2, d0, d1, d2); BiQuad L3( c1, c2, d0, d1, d2); const float T = 0.001f; // EMG const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array double StoreArray0[sizeMovAg] = {}, StoreArray1[sizeMovAg] = {}, StoreArray2[sizeMovAg] = {}; //Empty arrays to calculate MovAgs double Average0, Average1, Average2; //Outcome of MovAg const int sizeCali = 500; //Size of array over which the Threshold will be calculated double StoreCali0[sizeCali] = {}, StoreCali1[sizeCali] = {}, StoreCali2[sizeCali] = {}; //Empty arrays to calculate means in calibration double Mean0, Mean1, Mean2; //Mean of maximum contraction, calculated in the calibration double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; //Thresholds for muscles 0 to 2 int g = 0; //Part of the switch void, where the current state can be changed int emg_calib=0; //After calibration this value will be 1, enabling the // MOTOR_CAL volatile double tower_1_position = 0.1; // the tower which he reaches first volatile double tower_end_position = 0.1; // the tower which he reaches second volatile double rotation_start_position = 0.1; // the position where the rotation will remain volatile double position; volatile float speed = 1.0; volatile int dir = 0; // RKI related const double Ts = 0.001;// sample frequency // Constants motor const double delta_t = 0.01; const double el_1 = 370.0 / 2.0; const double el_2 = 65.0 / 2.0; const double pi = 3.14159265359; const double alpha = (2.0 * pi) /(25.0*8400.0); const double beta = (((2.0 * el_1) - (2.0 * el_2)) * 20.0 * pi) / (305.0 * 8400.0); const double q1start = rotation_start_position * alpha; const double q2start = tower_1_position * beta; const double q2end = tower_end_position * beta; // Variables motors volatile double desired_x; volatile double desired_y; volatile double out1; volatile double out2; volatile double vdesx; volatile double vdesy; volatile double q1; volatile double q2; volatile double MPe; volatile double xe; volatile double ye; volatile double gamma; volatile double dq1; volatile double dq2; volatile double dC1; volatile double dC2; volatile double pwm1; volatile double pwm2; // PID rotation constants volatile double Rot_Kp = 1.5; volatile double Rot_Ki = 0.1; volatile double Rot_Kd = 0.48; volatile double Rot_error = 0.0; volatile double Rot_prev_error = 0.0; // PID translation constants const double Trans_Kp = 0.5; const double Trans_Ki = 0.5; const double Trans_Kd = 0.1; volatile double Trans_error = 0.0; volatile double Trans_prev_error = 0.0; // states enum states {WAIT, MOTOR_CAL, EMG_CAL, START, OPERATING}; // states the robot can be in states CurrentState = WAIT; // the CurrentState to start with is the WAIT state bool StateChanged = true; // the state must be changed to go into the next state enum direction {Pos_RB, Pos_LB, Pos_RO, Pos_LO}; direction currentdirection = Pos_RB; bool directionchanged = true; // Functions // ----------------------------------------------------------------------------- // Encoder // Getting encoder information from motors int Counts1(volatile int& a) // a = counts1 { counts1_prev = a; a = Encoder1.getPulses(); return a; } int Counts2(volatile int& a) // a = counts2 { counts2_prev = a; a = Encoder2.getPulses(); return a; } // Servo control // To lift the pen up, with a push of button void servocontrol() { if(button_motorcal == false) // If button is pushed, pen should go up { myservo = 0.1; } myservo = 0.0; } // EMG filter // To process the EMG signal before information can be caught from it // Filter of the first EMG signal void filtering() { // Reading the EMG signal emg0 = emg0_in.read(); emg1 = emg1_in.read(); emg2 = emg2_in.read(); // Applying a notch filter over the EMG data notch0 = N1.step(emg0); notch1 = N2.step(emg1); notch2 = N3.step(emg2); // Applying a high pass filter high0 = H1.step(notch0); high1 = H2.step(notch1); high2 = H3.step(notch2); // Rectifying the signal absolute0 = fabs(high0); absolute1 = fabs(high1); absolute2 = fabs(high2); // Applying low pass filter low0 = L1.step(absolute0); low1 = L2.step(absolute1); low2 = L3.step(absolute2); } // Moving average filter // To determine the moving average, apply after filtering void MovAg() { // For statement to make an array of the last datapoints of the filtered signal for (int i = sizeMovAg - 1; i >= 0; i--) { // Shifts the i'th element one place to the right StoreArray0[i] = StoreArray0[i-1]; StoreArray1[i] = StoreArray1[i-1]; StoreArray2[i] = StoreArray2[i-1]; } // Stores the latest datapoint in the first element of the array StoreArray0[0] = low0; StoreArray1[0] = low1; StoreArray2[0] = low2; sum1 = 0.0; sum2 = 0.0; sum3 = 0.0; // For statement to sum the elements in the array for (int a = 0; a<=sizeMovAg-1; a++) { sum1+=StoreArray0[a]; sum2+=StoreArray1[a]; sum3+=StoreArray2[a]; } // Calculates an average over the datapoints in the array Average0 = sum1/sizeMovAg; Average1 = sum2/sizeMovAg; Average2 = sum3/sizeMovAg; // Sending the signal to the HIDScope // Change the number of channels in the beginning of the script when necessary scope.set( 0, emg0); scope.set( 1, low0); scope.set( 2, Average0); scope.set( 3, low1); scope.set( 4, emg2); scope.set( 5, low2); scope.send(); } // This must be applied to all emg signals coming in void processing_emg() { filtering(); MovAg(); } // WAIT // To do nothing void wait_mode() { // go back to the initial values // Copy here the variables list with initial values // all pwm's to zero // all counts to zero } // MOTOR_CAL // To calibrate the motor angle to some mechanical boundaries // Kenneth mee bezig void pos_store(int a){ //store position in counts to know count location of the ends of bridge if (tower_1_position == 0.1) { tower_1_position = a; } else if (tower_end_position == 0.1) { tower_end_position = a; } else if (rotation_start_position == 0.1) { rotation_start_position = a; } } // Start translation void translation_start(int a, float b) // a = dir , b = speed { dirpin_1.write(a); pwmpin_1 = b; } // Stop translation void translation_stop() { pwmpin_1 = 0.0; } // Start rotation void rotation_start(int a, float b) { dirpin_2.write(a); pwmpin_2 = b; } // Stop rotation void rotation_stop() { pwmpin_2 = 0.0; } // Calibration of translation void calibration_translation() { for(int m = 1; m <= 2; m++) // to do each direction one time { // dir = 0, means that the pen moves to the translation motor, dir = 1 means it moves to the rotation motor pc.printf("\r\nTranslatie loop\r\n"); translation_start(dir,speed); pc.printf("Direction = %i\r\n", dir); bool g = true; // to make a condition for the while loop while (g == true) { if (button_demo == false) // if button_demo is pushed, the translation should stop and change direction { translation_stop(); pos_store(Counts1(counts1)); pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position); dir = dir + 1; g = false; // to end the while loop } wait(0.01); } wait(1.0); // wait 3 seconds before next round of translation/rotation } } void calibration_rotation() { rotation_start(dir, speed); pc.printf("\r\nRotatie start\r\n"); bool f = true; // condition for while loop while(f == true) { if (button_motorcal == false) // If button_motorcal is pushed, then the motor should stop and remain in that position until homing { rotation_stop(); f = false; // to end the while loop } wait(0.01); } int start_counts = 0; pos_store(Counts2(start_counts)); pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position); } void motor_calibration() { // translation calibration_translation(); pc.printf("before wait\r\n"); wait(1.5); // rotation calibration_rotation(); pc.printf("Motor calibration done"); } // EMG_CAL // To calibrate the EMG signal to some boundary values // Void to switch between signals to calibrate void switch_to_calibrate() { g++; //If g = 0, led is blue if (g == 0) { led_blue==0; led_red==1; led_green==1; } //If g = 1, led is red else if(g == 1) { led_blue==1; led_red==0; led_green==1; } //If g = 2, led is green else if(g == 2) { led_blue==1; led_red==1; led_green==0; } //If g > 3, led is white else { led_blue==0; led_red==0; led_green==0; emg_calib = 0; g = 0; } } // Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required void calibrate() { switch (g) { case 0: { // Case zero, calibrate EMG signal of right biceps sum = 0.0; //For statement to make an array of the latest datapoints of the filtered signal for (int j = 0; j<=sizeCali-1; j++) { StoreCali0[j] = low0; // Stores the latest datapoint in the first element of the array sum+=StoreCali0[j]; // Sums the elements in the array wait(0.001f); } Mean0 = sum/sizeCali; // Calculates the mean of the signal Threshold0 = 1.5*Mean0; // Factor *2 is for resting and *1 is for max contraction break; } case 1: { // Case one, calibrate EMG signal of left biceps sum = 0.0; for(int j=0; j<=sizeCali-1; j++) { StoreCali1[j] = low1; sum+=StoreCali1[j]; wait(0.001f); } Mean1 = sum/sizeCali; Threshold1 = 2.0*Mean1; // Factor *2 is for resting and *1 is for max contraction break; } case 2: { // Case two, calibrate EMG signal of calf sum = 0.0; for(int j=0; j<=sizeCali-1; j++) { StoreCali2[j] = low2; sum+=StoreCali2[j]; wait(0.001f); } Mean2 = sum/sizeCali; Threshold2 = 2.0*Mean2; //Factor *2 is for resting and *1 is for max contraction break; } case 3: { // Sets calibration value to 1; robot can be set to Home position emg_calib=1; wait(0.001f); break; } default: { // Ensures nothing happens if g is not equal to 0, 1 or 2. break; } } } // Void to calibrate the EMG signal void emg_calibration() { for(int m = 1; m <= 4; m++) { led_blue == 0; led_red == 1; led_green == 1; pc.printf("g is %i\n\r",g); pc.printf("Average0 = %f , Average1 = %f, Average2 = %f \n\r",Average0, Average1, Average2); pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2); bool k = true; while(k == true) { if(button_motorcal == false) { calibrate(); // Calibrate threshold for 3 muscles k = false; } wait(0.2f); // Wait to avoid bouncing of button } bool h = true; while(h == true) { if (button_demo == false) { switch_to_calibrate(); // Switch state of calibration (which muscle) h = false; } wait(0.2f); // Wait to avoid bouncing of button } } // Turning all leds off led_red == 1; led_blue == 1; led_green == 1; } // START // To move the robot to the starting position: middle void start_mode() { // move to middle, only motor1 has to do something, the other one you can move yourself during the calibration int a = tower_end_position - ((tower_end_position - tower_1_position)/2); pc.printf("position middle = %i, position pen = %i \r\n", a, Counts1(counts1)); //translation home if (Counts1(counts1) > a) { translation_start(1,1.0); pc.printf("start to 1 \r\n"); } else { translation_start(0,1.0); pc.printf("start to 0 \r\n"); } while(true){ if ((Counts1(counts1) > (a - 500)) && (Counts1(counts1) < (a + 500))) { translation_stop(); pc.printf("stop \r\n"); break; } } } // OPERATING // To control the robot with EMG signals // Function for using muscle for direction control void Directioncontrol() { switch (currentdirection) { case Pos_RB: out1 = out1; out2 = out2; directionchanged = false; led_red == 0; if (Average2 > Threshold2) { currentdirection = Pos_LB; pc.printf("\r\n direction = Pos_LB\r\n"); directionchanged = true; } wait(0.5); // wait 0.5 seconds, otherwise it goes directly to the next case break; case Pos_LB: out1 = out1 * -1.0; out2 = out2; directionchanged = false; led_blue == 0; if (Average2 > Threshold2) { currentdirection = Pos_RO; pc.printf("\r\n direction = Pos_RO\r\n"); directionchanged = true; } wait(0.5); break; case Pos_RO: out1 = out1; out2 = out2 * -1.0; directionchanged = false; led_green == 0; if (Average2 > Threshold2) { currentdirection = Pos_LO; pc.printf("\r\n direction = Pos_LO\r\n"); directionchanged = true; } wait(0.5); break; case Pos_LO: out1 = out1 * -1.0; out2 = out2 * -1.0; directionchanged = false; led_red == 0; led_blue == 0; led_green == 0; if (Average2 > Threshold2) { currentdirection = Pos_RB; pc.printf("\r\n direction = Pos_RB\r\n"); directionchanged = true; } wait(0.5); break; } } // PID controller // To control the input signal before it goes into the motor control // PID execution double PID_control(volatile double error, const double kp, const double ki, const double kd, volatile double &error_int, volatile double &error_prev) { // P control double u_k = kp * error; // I control error_int = error_int + (Ts * error); double u_i = ki * error_int; // D control double error_deriv = (error - error_prev); double u_d = kd * error_deriv; error_prev = error; return u_k + u_i + u_d; } void boundaries() { double q2tot = q2 + dq2; if (q2tot > q2end) { dq2 = 0; } //kan ook zeggen q2end-q2 is dat dan juiste waarde of moet q2-q2end? else if (q2tot < q2start) { dq2 = 0; } } void motor_control() { // servocontrol(); // make sure the servo is used in this mode, maybe attach to a ticker? // filtering emg if (Average0 > Threshold0) { desired_x = 1.0; } else { desired_x = 0.0; } if (Average1 > Threshold1) { desired_y = 1.0; } else { desired_y = 0.0; } // calling functions // motor control out1 = desired_x; //control x-direction out2 = desired_y; //control y-direction Directioncontrol(); vdesx = out1 * 20.0; //speed x-direction vdesy = out2 * 20.0; //speed y-direction q1 = Counts2(counts2) * alpha + q1start; //counts to rotation (rad) q2 = Counts1(counts1)* beta + q2start; //counts to translation (mm) MPe = el_1 - el_2 + q2; //x location end effector, x-axis along the translation xe = cos(q1) * MPe; //x location in frame 0 ye = sin(q1) * MPe; //y location in frame 0 gamma = 1.0 /((-1.0 * ye * sin(q1)) - (xe * cos(q1))); //(1 / det(J'')inverse) dq1 = gamma * delta_t * (sin(q1) * vdesx - cos(q1) * vdesy); //target rotation dq2 = gamma * delta_t * (xe * vdesx + ye * vdesy) * -1.0; //target translation //boundaries(); dC1 = PID_control( dq1, Rot_Kp, Rot_Ki, Rot_Kd, Rot_error, Rot_prev_error) / alpha; //target rotation to counts dC2 = PID_control( dq2, Trans_Kp, Trans_Ki, Trans_Kd, Trans_error, Trans_prev_error) / beta; //target translation to counts pwm1 = 3.0 * (dC1 / delta_t) / 8400.0; // pwm2 = 7.0 * (dC2 / delta_t) / 8400.0; // dirpin_1.write(pwm2 < 0); // translatie pwmpin_1 = fabs (pwm2); dirpin_2.write(pwm1 < 0); // rotatie pwmpin_2 = fabs (pwm1); } // DEMO // To control the robot with a written code and write 'HELLO' // Voor het laatst bewaren void demo_mode() { // code here } // FAILURE // To shut down the robot after an error etc void failure_mode() { if (button_emergency == false) // condition for MOTOR_CAL --> FAILURE; button_emergency press { led_red == 0; // turning red led on to show emergency mode // all pwmpins zero pwmpin_1 = 0.0; pwmpin_2 = 0.0; // Servo up? // myservo = 0.1; pc.printf("\r\nEmergency mode, reset system to continue\r\n"); } } // Main function // ----------------------------------------------------------------------------- int main() { pc.baud(115200); // For TeraTerm, the baudrate, set also in TeraTerm itself! pc.printf("Start code\r\n"); // To check if the program starts pwmpin_1.period_us(60); // Setting period for PWM process_tick.attach(&processing_emg,T); // EMG signals must be filtered all the time! EMG signals filtered every T sec. emergency.attach(&failure_mode,0.01); // To make sure you can go to emergency mode all the time while(true){ // timer clock_t start; // start the timer start = clock(); time_overall = (clock() - start) / (double) CLOCKS_PER_SEC; myservo = 0.1; // Keep the pen lifted until servo function is called (operation mode) //pc.printf("potmeter value = %f ", potmeter_value); //pc.printf("counts = %i\r\n", counts); // With the help of a switch loop and states we can switch between states and the robot knows what to do switch(CurrentState) { case WAIT: if(StateChanged) // so if StateChanged is true { pc.printf("\r\nState is WAIT\r\n"); // Execute WAIT mode wait_mode(); StateChanged = false; // the state is still WAIT } if(button_motorcal == false) // condition for WAIT --> MOTOR_CAl; button_motorcal press { CurrentState = MOTOR_CAL; pc.printf("\r\nState is MOTOR_CAL\r\n"); StateChanged = true; } break; case MOTOR_CAL: if(StateChanged) // so if StateChanged is true { // Execute MOTOR_CAL mode motor_calibration(); StateChanged = false; // the state is still MOTOR_CAL } if((pwmpin_1 < 0.01) && (pwmpin_2 < 0.01)) // condition for MOTOR_CAL --> EMG_CAL; 3s and motors stopped moving { CurrentState = EMG_CAL; pc.printf("\r\nState is EMG_CAL\r\n"); StateChanged = true; } break; case EMG_CAL: if(StateChanged) // so if StateChanged is true { // Execute EMG_CAL mode emg_calibration(); StateChanged = false; // state is still EMG_CAL } if((Average0 < 0.04) && (Average1 < 0.04) && (Average2 < 0.04)) // condition for EMG_CAL --> START; 5s and EMG is low { CurrentState = START; pc.printf("\r\nState is START\r\n"); StateChanged = true; } break; case START: if(StateChanged) // so if StateChanged is true { // Execute START mode start_mode(); pc.printf("pwmpin_1 = %f pwmpin_2 = %f \r\n", pwmpin_1, pwmpin_2); StateChanged = false; // state is still START } if((pwmpin_1 < 0.01) && (pwmpin_2 < 0.01)) // condition for START --> OPERATING; 5s and motors stopped moving { CurrentState = OPERATING; pc.printf("\r\nState is OPERATING\r\n"); StateChanged = true; } break; case OPERATING: if(StateChanged) // so if StateChanged is true { // Execute OPERATING mode while(true) { motor_control(); pc.printf("PWM_rot = %f PWM_trans = %f VdesX = %f VdesY = %f \n\r",pwm1,pwm2,vdesx,vdesy); pc.printf("out1 = %f out2 = %f \n\r", out1, out2); //pc.printf("Average0 = %f Average1 = %f Average2 = %f\r\n", Average0,Average1,Average2); if(button_wait == false) // condition OPERATING --> WAIT; button_wait press { CurrentState = WAIT; pc.printf("\r\nState is WAIT\r\n"); StateChanged = true; break; } wait(delta_t); } StateChanged = false; // state is still OPERATING } break; // no default } // while loop does not have to loop every time } }