Koen Lotze
kinematic model included
Dependencies: Encoder MODSERIAL biquadFilter
Diff: f8f7934e8460/main.cpp
- Revision:
- 0:ef7fd98bf091
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/f8f7934e8460/main.cpp Thu Oct 26 14:20:40 2017 +0000
@@ -0,0 +1,369 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "MODSERIAL.h"
+#include "BiQuad.h"
+#include "math.h"
+
+#define M_Pi 3.141592653589793238462643383279502884L
+
+Serial pc(USBTX, USBRX);
+
+DigitalOut led_red(LED_RED);
+DigitalOut led_blue(LED_BLUE);
+InterruptIn button1(D2);
+InterruptIn button2(D3);
+AnalogIn EMG_A0(A0);
+AnalogIn EMG_A1(A1);
+
+DigitalOut motor1DirectionPin(D4);
+PwmOut motor1MagnitudePin(D5);
+DigitalOut motor2DirectionPin(D7); // Sequence? Lines 181-183
+PwmOut motor2MagnitudePin(D6);
+
+Ticker measureTick;
+
+Encoder motor1(D13,D12); //On the shield actually M2
+Encoder motor2(D11,D10); //On the shield actually M1 (Production mistake?)
+
+int Count1 = 0;
+int Count2 = 0;
+int MaxEMGcount = 1800; // Split up for motor 1 and 2 AND create a MinEMGCount
+int MinEMGcount = -1800;
+bool switch1 = 0; // manual switch for when to start calculations (later removed for a state machine
+bool direction1 = 1;
+bool direction2 = 1;
+const double RAD_PER_PULSE = 0.002991; // Value for RAD_PER_PULSE given through the slides (wrong?)
+const double DEG_PER_RAD = 180 / M_Pi; // Basic knowledge of how many degrees are in 1 radian.
+int MaxAveragerValues = 500;
+double Av_A0 = 0;
+double Av_A1 = 0;
+double q1 = 0; // Angle of arm 1 (upper) in starting position is 0 degrees
+double q2 = 179/DEG_PER_RAD; // Angle of arm 2 (lower) in starting position is 180 degrees (but can't be 0 or 180 because of determinant = 0)
+int L1 = 47; // Length of arm 1 (upper) in cm
+int L2 = 29; // Length of arm 2 (lower) in cm
+int xdes = L1-L2 // Desired x coordinate, arm is located at x = L1-L2 in starting position
+int ydes = 0; // Disired y coordinate, arm is located at y = 0 in starting position
+
+// Sample time (motor1-timestep)
+const double M1_Ts = 0.01; //100 Hz systems
+const double M2_Ts = 0.01;
+
+// Controller gains (motor1-Kp,-Ki,...)
+const double M1_Kp = 0.1, M1_Ki = 0.02, M1_Kd = 0.00125, M1_N = 100; // THESE VALUES ARE ARBITRARY AT THIS POINT
+const double M2_Kp = 0.1, M2_Ki = 0.02, M2_Kd = 0.00125, M2_N = 100; // Inspired by the Ziegler-Nichols Method
+
+// Filter variables (motor1-filter-v1,-v2)
+double M1_f_v1 = 0.0, M1_f_v2 = 0.0;
+double M2_f_v1 = 0.0, M2_f_v2 = 0.0;
+
+// PROGRAM THAT CALCULATES ANGLE CHANGES
+
+
+// PROGRAM THAT ANALYSES EMG SIGNAL AND FILTERS IT
+// Define the HIDScope and Ticker object
+//HIDScope scope(2);
+//Ticker scopeTimer;
+
+BiQuadChain bqchain1, bqchain2; //BiQuad Chain. Makes a combination of various BiQuads.
+
+BiQuad bq1(0.960582636413903, -1.921165272827806, 0.960582636413903, -1.916303032724640, 0.926027512930974);//Highpass 8Hz
+BiQuad bq2(0.912835456766912, -1.825670913533825, 0.912835456766912, -1.821050358261190, 0.830291468806461);//
+BiQuad bq3(0.991153589776121, -1.603846829332603, 0.991153589776121, -1.593257429417890, 0.982171881701343);//Notch 49-51Hz
+BiQuad bq4(0.991153589776121, -1.603846829332603, 0.991153589776121, -1.614311809680850, 0.982599066293185);//
+BiQuad bq5(0.064591432448980, 0.129182864897960, 0.064591432448980, -1.401313635492332, 0.659679365288253);//Lowpasss 45Hz
+BiQuad bq6(0.052062866562661, 0.104125733125322, 0.052062866562661, -1.129505912021716, 0.337757378272360);//
+
+BiQuadChain bqchain3, bqchain4; //BiQuad Chain. Makes a combination of various BiQuads.
+
+BiQuad bq7(0.960582636413903, -1.921165272827806, 0.960582636413903, -1.916303032724640, 0.926027512930974);//Highpass 8Hz
+BiQuad bq8(0.912835456766912, -1.825670913533825, 0.912835456766912, -1.821050358261190, 0.830291468806461);//
+BiQuad bq9(0.991153589776121, -1.603846829332603, 0.991153589776121, -1.593257429417890, 0.982171881701343);//Notch 49-51Hz
+BiQuad bq10(0.991153589776121, -1.603846829332603, 0.991153589776121, -1.614311809680850, 0.982599066293185);//
+BiQuad bq11(0.064591432448980, 0.129182864897960, 0.064591432448980, -1.401313635492332, 0.659679365288253);//Lowpasss 45Hz
+BiQuad bq12(0.052062866562661, 0.104125733125322, 0.052062866562661, -1.129505912021716, 0.337757378272360);//
+
+double A0t10=0, A0t9=0, A0t8=0, A0t7=0, A0t6=0, A0t5=0, A0t4=0, A0t3=0, A0t2=0, A0t1=0;
+double A1t10=0, A1t9=0, A1t8=0, A1t7=0, A1t6=0, A1t5=0, A1t4=0, A1t3=0, A1t2=0, A1t1=0;
+
+double EMGSample1(double Input, double &t10, double &t9, double &t8, double &t7, double &t6, double &t5, double &t4, double &t3, double &t2, double &t1){ // Sample function: read a value from the Analog in
+ //double MaxSignal = 0;
+ double Signal = 0.0f;
+ double EMGFiltered = fabs(bqchain1.step(Input)); // Signal.Read instantly gets filtered.
+ double EMGSignal = bqchain2.step(EMGFiltered);
+ double treshhold = 0.6*Av_A0;
+ // if (EMGSignal > MaxSignal){ MaxSignal = EMGSignal;} // Save the highest value measured.
+ // double discrete = floor((EMGSignal/MaxSignal)*10.0)/10.0;
+ if (EMGSignal >= treshhold || t1 >= treshhold || t2 >= treshhold || t3 >= treshhold || t4 >= treshhold || t5 >= treshhold || t6 >= treshhold || t7 >= treshhold || t8 >= treshhold || t9 >= treshhold || t10 >= treshhold ){
+ Signal = 1;
+ }
+ else {
+ Signal = 0.0f;
+ }
+ t10=t9, t9=t8, t8=t7, t7=t6, t6=t5, t5=t4, t4=t3, t3=t2, t2=t1, t1= EMGSignal;
+
+ return Signal;
+}
+
+double EMGSample2(double Input, double &t10, double &t9, double &t8, double &t7, double &t6, double &t5, double &t4, double &t3, double &t2, double &t1){ // Sample function: read a value from the Analog in
+ //double MaxSignal = 0;
+ double Signal = 0.0f;
+ double EMGFiltered = fabs(bqchain3.step(Input)); // Signal.Read instantly gets filtered.
+ double EMGSignal = bqchain4.step(EMGFiltered);
+ double treshhold = 0.6*Av_A1;
+ // if (EMGSignal > MaxSignal){ MaxSignal = EMGSignal;} // Save the highest value measured.
+ // double discrete = floor((EMGSignal/MaxSignal)*10.0)/10.0;
+ if (EMGSignal >= treshhold || t1 >= treshhold || t2 >= treshhold || t3 >= treshhold || t4 >= treshhold || t5 >= treshhold || t6 >= treshhold || t7 >= treshhold || t8 >= treshhold || t9 >= treshhold || t10 >= treshhold ){
+ Signal = 1;
+ }
+ else {
+ Signal = 0.0f;
+ }
+ t10=t9, t9=t8, t8=t7, t7=t6, t6=t5, t5=t4, t4=t3, t3=t2, t2=t1, t1= EMGSignal;
+
+ return Signal;
+}
+
+// PROGRAM THAT MEASURES AN AMOUNT OF VALUES AND EXPORTS THE AVERAGE
+void AverageValue_A0(double &Av_A0)
+{
+ double A[MaxAveragerValues];
+ double sum = 0;
+ for( int i = 0; i < MaxAveragerValues; i++){
+ double EMGFiltered = fabs(bqchain1.step(EMG_A0.read())); // Signal.Read instantly gets filtered.
+ double Value = bqchain2.step(EMGFiltered);
+ wait(0.01);
+ for (int j = 1; j < MaxAveragerValues; j++){
+ A[MaxAveragerValues-j] = A[MaxAveragerValues-1-j];
+ }
+ A[0] = Value;
+ }
+ for (int i = 0; i < MaxAveragerValues; i++){
+ sum = sum + A[i];
+ }
+ printf("result = %.0f", (sum/MaxAveragerValues));
+ Av_A0 = (sum/MaxAveragerValues);
+}
+
+void AverageValue_A1(double &Av_A1)
+{
+ double A[MaxAveragerValues];
+ double sum = 0;
+ for( int i = 0; i < MaxAveragerValues; i++){
+ double EMGFiltered = fabs(bqchain3.step(EMG_A1.read()));
+ double Value = bqchain4.step(EMGFiltered);
+ wait(0.01);
+ for (int j = 1; j < MaxAveragerValues; j++){
+ A[MaxAveragerValues-j] = A[MaxAveragerValues-1-j];
+ }
+ A[0] = Value;
+ }
+ for (int i = 0; i < MaxAveragerValues; i++){
+ sum = sum + A[i];
+ }
+ printf("result = %.0f", (sum/MaxAveragerValues));
+ Av_A1 = (sum/MaxAveragerValues);
+}
+
+// PROGRAM THAT ENABLES THE SYSTEM TO OPERATE (Later to be replaced by State Machine)
+void but1_pressed(void)
+{
+ if (switch1 == 0){
+ pc.printf("TAKING AVERAGES...");
+ for(int i = 1; i < 12; i++){
+ led_blue = !led_blue;
+ wait(0.1);
+ }
+ AverageValue_A0(Av_A0);
+ pc.printf("Av_A0 = %.3f ", Av_A0);
+ led_blue = 1;
+ for(int i = 1; i < 12; i++){
+ led_blue = !led_blue;
+ wait(0.1);
+ }
+ AverageValue_A1(Av_A1);
+ pc.printf("Av_A1 = %.3f \r\n\n", Av_A1);
+ led_blue = 1;
+ }
+ wait(10.0);
+ led_red = !led_red;
+ switch1 = !switch1;
+}
+
+void but2_pressed(void)
+{
+ direction1 = !direction1;
+ direction2 = !direction2;
+}
+
+// PROGRAM THAT CALCULATES THE PID
+double PID( double err, const double Kp, const double Ki, const double Kd,
+const double Ts, const double N, double &v1, double &v2 ) {
+
+ const double a1 = -4/(N*Ts+2), a2 = -(N*Ts-2)/(N*Ts+2), // a1 and a2 are the nominator of our transferfunction
+ b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
+ b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
+ b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4); // b0, b1 and b2 the denominator
+
+ double v = err - a1*v1 - a2*v2; // Memory value are calculated and later on stored. (v is like an input)
+ double u = b0*v + b1*v1 + b2*v2;
+ v2 = v1; v1 = v;
+ return u; // u functions as our output value gained from the transferfunction.
+}
+
+// PROGRAM THAT RECEIVES EMG SIGNALS AND CONVERTS THEM TO REFERENCE POSITIONS
+double EMG_Referencer(double sig, bool dir, int &n) {
+ //pc.printf("MEASURING");
+ if (dir == 1){
+ if (sig > 0.99){ //Above 0.9 because of inconsistency in received signals
+ if (n < MaxEMGcount){
+ n = n++; //Normally in steps of 1 (so n = n++;) but for better visibility we have 36 itterations
+ }
+ }
+ }
+ else if (dir == 0){
+ if (sig > 0.99){
+ if (n > MinEMGcount){ //MinEMGcount
+ n = n--; // Normally n = n--;
+ }
+ }
+ }
+ double u = 0.1*n;
+ return u;
+}
+
+//Kinematic model
+double Kinematic_referencer(double xdes, bool ydes, int &n) { //weet niet wat ik met die n moet
+ //printf ("x=%f, y=%f\n", x, y);
+ double qt = q1 + q2; // current total angle
+ double xcurrent = L1 * cos (q1) + L2 * cos (qt); // current x position
+ double ycurrent = L1 * sin (q1) + L2 * sin (qt); // current y position
+
+ //Initial twist
+ double vx = (xdes - xcurrent)/100; // Running on 100 Hz
+ double vy = (ydes - ycurrent)/100;
+
+ //Jacobians
+ double J11 = -ycurrent;
+ double J12 = -L2 * sin (qt);
+ double J21 = xcurrent;
+ double J22 = L2 * cos (qt);
+ double Determinant = J11 * J22 - J21 * J12; // calculate determinant
+
+ //Calculate angular velocities
+ double q1der = (J22 * vx - J12 * vy) / Determinant;
+ double q2der = (-J21 * vx + J11 * vy) / Determinant;
+
+ //Calculate new angles
+ q1new = q1 + q1der/100; //nog fixen met die tijdstappen?
+ q2new = q2 + q2der/100; //hier ook
+ //printf ("q1=%f, q2=%f\n", q1 * c, q2 * c);
+ qtnew = q1new + q2new;
+
+ //Calculate new positions
+ xnew = L1 * cos (q1new) + L2 * cos (qtnew);
+ ynew = L1 * sin (q1new) + L2 * sin (qtnew);
+ //printf ("x=%f, y=%f\n", x, y);
+
+ // Now check whether the calculated position is desired, determinants close to zero may cause the robot to move weirdly
+ // New y may not be negative, this means the arm is located in on the plate
+ // New q1 may not be less than -45 degrees, less means the arm will crash into the base plate
+ // New q2 may not be more than 185 degrees, more means the lower arm will crash into the upper arm
+ if (ynew > -1 && q1new > -45 / DEG_PER_RAD && q2new < 185 / DEG_PER_RAD)
+ {
+ // If desired, change the angles
+ q1 = q1new;
+ q2 = q2new;
+ }
+ else
+ {
+ // If not desired, don't change the angles, but define current position as desired so the robot ignores the input
+ xdes = xcurrent;
+ ydes = ycurrent;
+ }
+ return q1,q2;
+}
+
+// PROGRAMS THAT CONTROL THE VALUE OUTPUT
+double M1_Controller(double Angle1) {
+ double EMG_Input_A0 = EMG_A0.read();
+ double sig1 = EMGSample1(EMG_Input_A0, A0t10, A0t9, A0t8, A0t7, A0t6, A0t5, A0t4, A0t3, A0t2, A0t1);
+
+ double referencePosition = EMG_Referencer(sig1, direction1, Count1);
+ pc.printf("D1 = %i, S1 = %.0f ", Count1, sig1);
+ double motor1Value = PID( referencePosition - Angle1 , M1_Kp, M1_Ki, M1_Kd, M1_Ts, M1_N, M1_f_v1, M1_f_v2); //Find the motorvalue by going through the PID
+
+ return motor1Value; // According to the PID its calculations, we get a motor value.
+}
+
+double M2_Controller(double Angle2) {
+ double EMG_Input_A1 = EMG_A1.read();; //EMG_A1.read();
+ double sig2 = EMGSample2(EMG_Input_A1, A1t10, A1t9, A1t8, A1t7, A1t6, A1t5, A1t4, A1t3, A1t2, A1t1);
+
+ double referencePosition = EMG_Referencer(sig2, direction2, Count2);; // Since we have 360 degrees, our potmeter should reach these values as well.
+ pc.printf("D2 = %i, S2 = %.0f \r\n", Count2, sig2);
+ double motor2Value = PID( referencePosition - Angle2 , M2_Kp, M2_Ki, M2_Kd, M2_Ts, M2_N, M2_f_v1, M2_f_v2); //Find the motorvalue by going through the PID
+
+ return motor2Value; // According to the PID its calculations, we get a motor value.
+}
+
+// PROGRAMS FOR POWERING THE MOTOR ACCORDING TO THE ERROR (P VARIANT)
+void SetMotor1(double motor1Value) // function that actually changes the output for the motor
+{
+ if(motor1Value >= 0 ) //Function sets direction and strength
+ motor1DirectionPin = 1; //If the reference value is positive, we will turn clockwise
+ else
+ motor1DirectionPin = 0; // if not, counterclockwise
+
+ if(fabs(motor1Value) > 0.2 ) // Next, check the absolute motor value, which is the magnitude
+ motor1MagnitudePin = 0.2; // This is a safety. We never want to exceed 1
+ else
+ motor1MagnitudePin = fabs(motor1Value); // if we fall underneath the safety, take the magnitude
+}
+
+void SetMotor2(double motor2Value) // function that actually changes the output for the motor
+{
+ if(motor2Value >= 0 ) //Function sets direction and strength
+ motor2DirectionPin = 0; //If the reference value is positive, we will turn clockwise
+ else
+ motor2DirectionPin = 1; // if not, counterclockwise
+
+ if(fabs(motor2Value) > 0.2 ) // Next, check the absolute motor value, which is the magnitude
+ motor2MagnitudePin = 0.2; // This is a safety. We never want to exceed 1
+ else
+ motor2MagnitudePin = fabs(motor2Value); // if we fall underneath the safety, take the magnitude
+}
+
+// PROGRAM THAT MEASURES AND CONTROLES THE MOTOR OUTPUT
+void MeasureAndControl() // Pure values being calculated and send to the Mbed.
+{
+ double Angle1 = DEG_PER_RAD * RAD_PER_PULSE * motor1.getPosition(); // Angle is equal to the degrees per pulse measured (NOT TRUE AT THIS MOMENT, '360' != 2 * M_Pi
+ double Angle2 = DEG_PER_RAD * RAD_PER_PULSE * motor2.getPosition(); // Angle is equal to the degrees per pulse measured (NOT TRUE AT THIS MOMENT, '360' != 2 * M_Pi
+
+ if (switch1 == 1){ // If the switch is pressed
+ double motor1Value = M1_Controller(Angle1); // create a motorvalue to give to motor 1 according to the control and angle
+ double motor2Value = M2_Controller(Angle2); // create a motorvalue to give to motor 1 according to the control and angle
+
+ SetMotor1(motor1Value);
+ SetMotor2(motor2Value);
+ }
+ else {
+ SetMotor1(0);
+ SetMotor2(0); }
+}
+
+int main() // Main function
+{
+ pc.baud(115200); // For post analysis, seeing if the plug works etc.
+ pc.printf("STARTING SEQUENCE \r\n"); //Merely checking if there is a serial connection at all
+ bqchain1.add(&bq1).add(&bq2).add(&bq3).add(&bq4); // Combine the BiQuads bq1, bq2 and bq3 in the Chain.
+ bqchain2.add(&bq5).add(&bq6);
+ bqchain3.add(&bq7).add(&bq8).add(&bq9).add(&bq10); // Combine the BiQuads bq1, bq2 and bq3 in the Chain.
+ bqchain4.add(&bq11).add(&bq12);
+ measureTick.attach(&MeasureAndControl, M1_Ts); // Tick that changes the motor (currently 1Hz)
+ led_red = 1; // Set the LED off in the positive direction, on in the negative direction
+ led_blue = 1;
+ button1.fall(&but1_pressed); // Trigger the InterruptIn of the button.
+ button2.fall(&but2_pressed);
+ //scopeTimer.attach_us(&scopeSend, 1e4); // Attach in Microseconds
+}
+