Revision 23:d13db573a875, committed 2019-10-31

Comitter:

sanou8

Date:

Thu Oct 31 20:36:14 2019 +0000

Parent:

22:08b3cd7bec7f

Commit message:

Totale code

Changed in this revision

diff -r 08b3cd7bec7f -r d13db573a875 FastPWM.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Thu Oct 31 20:36:14 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Sissors/code/FastPWM/#c0b2265cff9c

diff -r 08b3cd7bec7f -r d13db573a875 Filter/FilterDesign.cpp
--- a/Filter/FilterDesign.cpp	Tue Oct 29 12:11:23 2019 +0000
+++ b/Filter/FilterDesign.cpp	Thu Oct 31 20:36:14 2019 +0000
@@ -4,7 +4,12 @@
 
 
 
-
+// Notch filter on 50 Hz
+double nb0 = 0.999103206817809;
+double nb1 = -1.994263409725146;
+double nb2 = 0.999103206817809;
+double na1 = -1.994263409725146;
+double na2 = 0.998206413635618;
 
 // 4th order Butterworth High pass 9 Hz
 double hpb0 = 0.862550850547179;
@@ -31,13 +36,14 @@
 // Multiplication with the gain
 double gain = 10.00000;     
 
-                        
+BiQuad notch50(nb0, nb1, nb2, na1, na2);                         
 BiQuad4 highpass(hpb0, hpb1, hpb2, hpb3, hpb4, hpa1, hpa2, hpa3, hpa4);
 BiQuad4 lowpass(lpb0, lpb1, lpb2, lpb3, lpb4, lpa1, lpa2, lpa3, lpa4);
 
 double FilterDesign(double u)
 {   
-    double y_hp = highpass.step(u);   // Secondly the highpassfilter
+    double y_n = notch50.step(u);       // First the notchfilter
+    double y_hp = highpass.step(y_n);   // Secondly the highpassfilter
     double y_abs = abs(y_hp);           // Make the signal values absolute
     double y_lp = lowpass.step(y_abs);  // Then a lowpass filter
     double y_gain = y_lp*gain;          // Multiply by a gain

diff -r 08b3cd7bec7f -r d13db573a875 QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Thu Oct 31 20:36:14 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

diff -r 08b3cd7bec7f -r d13db573a875 main.cpp
--- a/main.cpp	Tue Oct 29 12:11:23 2019 +0000
+++ b/main.cpp	Thu Oct 31 20:36:14 2019 +0000
@@ -4,68 +4,94 @@
 #include "BiQuad.h"
 #include "BiQuad4.h"
 #include "MODSERIAL.h"
+#include "QEI.h"
+#include <vector>
+
+using std::vector;
 
 Serial pc(USBTX,USBRX);
-DigitalIn button(SW3) ;
-
+DigitalIn button_motor(SW2) ;
+DigitalIn button_demo(D9) ;
+DigitalIn button_emg(D8);
+DigitalIn Fail_button(SW3);
+InterruptIn sw2(SW2);
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+DigitalOut led_blue(LED_BLUE);
 
 //Define objects
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
-AnalogIn potmeter1(PTC11);          // Input of two potmeters
-AnalogIn potmeter2(PTC10);
+QEI Enc1(D12, D13, NC, 32);
+QEI Enc2(D10, D11, NC, 32);
+DigitalOut M1(D4);
+DigitalOut M2(D7);
+PwmOut E1(D5);
+PwmOut E2(D6);
 
-
+Ticker      StateMachine;
 Ticker      ticker_calibration;   // Ticker to send the EMG signals to screen
 Ticker      sample_timer;           // Ticker for reading out EMG
 HIDScope    scope( 2 );
-DigitalOut  led(LED1);
+DigitalOut  led(LED_RED);
+DigitalOut  controlLED(LED_GREEN);
+Timer timer;
 
 volatile double emg1_filtered;      //measured value of the first emg
 volatile double emg2_filtered;      //measured value of the second emg
-volatile double emg1_max ;           // calibrated value of first emg
-volatile double emg2_max ;
 volatile double emg1_cal = 0.8;
- 
- // Read EMG
-//void EMGread()
-//{
-//    emg1_filtered = FilterDesign(emg0.read());
-//    emg2_filtered = FilterDesign(emg1.read());
-    //pc.printf("emg1_cal = %f, emg2_cal = %f \n\r", emg1_filtered, emg2_filtered);
-//}
+double Pi = 3.14159265359;
+double gearRatio = 40/9;
+double time_in_state ;
 
+double initRot1 = 0;
+double initRot2 = -gearRatio*(180 - 48.5)/360;
+volatile double y_new = 20.0 ;
+volatile double y_prev = 20.0 ;
+double displacement = 1 ;
+double speedy = 3 ;
 
- 
+enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, OPERATION, SHOOTING, MOVE_W_DEMO, FAILURE_MODE};
+states currentState = WAITING;      // Start in waiting state
+bool stateChanged = true; 
 
 void sample() ;
-void EMGcalibration () ;
+void EMGcalibration() ;
+void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes);
+double calcRot1(double xDes, double yDes);
+double calcRot2(double xDes, double yDes);
+void plotPath(double xStart, double yStart, double xEnd, double yEnd, double *xPath[], double *yPath[]);
+void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed);
+void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes);
+double calcX(double rot1, double rot2);
+double calcY(double rot1, double rot2);
+void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed);
+void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation);
+void DEMO_motor() ;
+void EMG_move();
+void state_machine(void);
+void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed);
+void moveMotorsToStop(DigitalOut *M1, PwmOut *E1, QEI *Enc1, double speed1, DigitalOut *M2, PwmOut *E2, QEI *Enc2, double speed2);
 
-
+void changespeed()
+{
+    speedy = speedy*-1;
+}
 
 
 int main()
-{   
-    /**Attach the 'sample' function to the timer 'sample_timer'.
-    * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
-    */
-    
+{
+    led_red = 1;
+    led_green = 1;
+    led_blue = 1;
+
     pc.baud(115200);
-    //EMGcalibration();
-    sample_timer.attach(&sample, 0.002);
-    
-   
-    /*empty loop, sample() is executed periodically*/
+
+
+StateMachine.attach(&state_machine, 0.005f);
     while(true) {
-        if(SW3==0){
-            EMGcalibration();
-            }
-            
-    led = 1; 
-        
-    while(emg1_filtered >= 0.9*emg1_cal){
-            led = 0;
-    }
+
+     
     }
 }
 
@@ -74,14 +100,14 @@
 
 
 
-void sample() 
+void sample()
 {
     emg1_filtered = FilterDesign(emg0.read());
     emg2_filtered = FilterDesign(emg1.read());
     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
     scope.set(0, emg1_filtered ) ;
     scope.set(1, emg2_filtered );
-    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
+    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
     *  Ensure that enough channels are available (HIDScope scope( 2 ))
     *  Finally, send all channels to the PC at once */
     scope.send();
@@ -90,15 +116,454 @@
     //led = !led;
 }
 
-void EMGcalibration ()
+void EMGcalibration()
+{
+
+    Timer tijd;
+    tijd.start();
+    do {
+        ticker_calibration.attach(&sample, 0.002);
+        if(emg1_cal < emg1_filtered) {
+            emg1_cal = emg1_filtered ;
+            pc.printf("EMG_cal : %g \n\r",emg1_cal);
+        }
+    } while(tijd<10);
+}
+
+
+//function to mave a motor to a certain number of rotations, counted from the start of the program.
+//parameters:
+//DigitalOut *M = pointer to direction cpntol pin of motor
+//PwmOut *E = pointer to speed contorl pin of motor
+//QEI *Enc = pointer to encoder of motor
+//double rotDes = desired rotation
+void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
+{
+    double Kp = 30;  //180 & 10 werkt zonder hulp
+    double Ki = 0;
+    double Kd = 2;
+
+    double pErrorC;
+    double pErrorP = 0;
+    double iError = 0;
+    double dError;
+
+    double U_p;
+    double U_i;
+    double U_d;
+
+    double rotC = Enc->getPulses()/(32*131.25) + initRot;
+    double MotorPWM;
+
+    Timer t;
+    double tieme = 0;
+
+    t.start();
+    do {
+        pErrorP = pErrorC;
+        rotC = Enc->getPulses()/(32*131.25) + initRot;
+        pErrorC = rotDes - rotC;
+        tieme = t.read();
+        t.reset();
+        iError = iError + pErrorC*tieme;
+        dError = (pErrorC - pErrorP)/tieme;
+
+        U_p = pErrorC*Kp;
+        U_i = iError*Ki;
+        U_d = dError*Kd;
+        MotorPWM = (U_p + U_i + U_d)*dir;
+
+        if(MotorPWM > 0) {
+            *M = 0;
+            *E = MotorPWM;
+        } else {
+            *M = 1;
+            *E = -MotorPWM;
+        }
+        wait(0.01);
+        //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
+    } while (abs(MotorPWM) > 0.13|| abs(dError > 0.01));; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01);
+    *E = 0;
+    //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
+    t.stop();
+}
+
+
+void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes)
+{
+    double Kp = 61;  //180 & 10 werkt zonder hulp
+    double Ki = 1;
+    double Kd = 7;
+
+    double rotC = Enc->getPulses()/(32*131.25) + initRot;
+
+    double pErrorC = rotDes - rotC;
+
+    double tieme = t->read();
+    double dt = tieme - pidInfo->at(2);
+
+    double iError = pidInfo->at(1) + pErrorC*dt;
+    double dError = (pErrorC - pidInfo->at(0))/dt;
+
+    double MotorPWM = (pErrorC*Kp + iError*Ki + dError*Kd)*dir;
+
+    if(MotorPWM > 0) {
+        *M = 0;
+        *E = MotorPWM;
+    } else {
+        *M = 1;
+        *E = -MotorPWM;
+    }
+    pidInfo->clear();
+    pidInfo->push_back(pErrorC);
+    pidInfo->push_back(iError);
+    pidInfo->push_back(tieme);
+    pidInfo->push_back(Enc->getPulses()/(32*131.25) + initRot);
+}
+
+
+//double that calculates the rotation of one arm.
+//parameters:
+//double xDes = ofset of the arm's shaft in cm in the x direction
+//double yDes = ofset of the arm's shaft in cm in the y direction
+double calcRot1(double xDes, double yDes)
+{
+    double alpha = atan(yDes/xDes);
+    if(alpha < 0) {
+        alpha = alpha+Pi;
+    }
+    //pc.printf("alpha: %f", alpha);
+    return gearRatio*((alpha - 0.5*(Pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*Pi));
+}
+
+double calcRot2(double xDes, double yDes)
+{
+    double alpha = atan(yDes/xDes);
+    if(alpha < 0) {
+        alpha = alpha+Pi;
+    }
+    return gearRatio*((alpha + 0.5*(Pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*Pi));
+}
+
+void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation)
+{
+    double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
+    double traveledDistance = speed * t->read();
+    double ratio = traveledDistance/pathLength;
+
+    desiredLocation->clear();
+    desiredLocation->push_back(xStart + (xEnd - xStart)*ratio);
+    desiredLocation->push_back(yStart + (yEnd - yStart)*ratio);
+
+}
+
+void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed)
+{
+    double rot1;
+    double rot2;
+
+    Timer t;
+
+    vector<double> desiredLocation;
+
+    vector<double> pidInfo1 (3);
+    vector<double> pidInfo2 (3);
+
+    fill(pidInfo1.begin(), pidInfo1.begin()+3, 0);
+    fill(pidInfo2.begin(), pidInfo2.begin()+3, 0);
+
+    double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
+
+    //Calculate the rotation of the motors at the start of the path
+    rot1 = calcRot1(xStart, yStart);
+    rot2 = calcRot2(xStart, yStart);
+    pc.printf("r1: %f r2: %f", rot1/6, rot2/6);
+
+    //Move arms to the start of the path
+    //moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
+    //moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
+
+    //start the timer
+    t.start();
+    while(pathLength > speed * t.read()) {
+        findDesiredLocation(xStart, yStart, xEnd, yEnd, speed, &t, &desiredLocation);
+        rot1 = calcRot1(desiredLocation.at(0), desiredLocation.at(1));
+        //pc.printf("\n\r Rot1: %f", rot1);
+        moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
+        rot2 = calcRot2(desiredLocation.at(0), desiredLocation.at(1));
+        pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2);
+        moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
+        wait(0.01);
+    }
+
+}
+
+
+
+double calcX(double rot1, double rot2)
+{
+    return 20*cos((rot1/gearRatio)*2*Pi)+20*cos((-rot2/gearRatio)*2*Pi);
+}
+
+double calcY(double rot1, double rot2)
+{
+    return 20*sin((rot1/gearRatio)*2*Pi)+20*sin((-rot2/gearRatio)*2*Pi);
+}
+
+
+void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed)
 {
-    
-Timer t;
-t.start();
-    do { 
-    ticker_calibration.attach(&sample, 0.002);
-    if(emg1_cal < emg1_filtered){
-        emg1_cal = emg1_filtered ;
+    Timer t;
+
+    vector<double> pidInfo1 (4);
+    vector<double> pidInfo2 (4);
+
+    fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
+    fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);
+
+    double xC = *xStart;
+    double yC = *yStart;
+
+    double rot1;
+    double rot2;
+
+    double tiemeP;
+    double tiemeC;
+    double dt;
+
+    t.start();
+
+    tiemeP = t.read();
+    while(t.read() < 0.1) {
+        tiemeC = t.read();
+        dt = tiemeC - tiemeP;
+        xC = xC+xSpeed*dt;
+        yC = yC+ySpeed*dt;
+        rot1 = calcRot1(xC, yC);
+        rot2 = calcRot2(xC, yC);
+        moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
+        moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
+        tiemeP = tiemeC;
+        wait(0.01);
+    }
+
+    *xStart = xC;//calcX(pidInfo1.at(3), pidInfo2.at(3));
+    *yStart = yC;//calcY(pidInfo1.at(3), pidInfo2.at(3));
+}
+
+void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed)
+{
+    Timer t;
+ 
+    double MotorPWM;
+ 
+    double posC;
+    double posP = Enc->getPulses()/(32*131.25);
+    double vel = 0;
+ 
+    int hasNotMoved = 0;
+ 
+ 
+    t.start();
+    do {
+        MotorPWM = speed - vel*0.7;
+        if(MotorPWM > 0) {
+            *M = 0;
+            *E = MotorPWM;
+        } else {
+            *M = 1;
+            *E = -MotorPWM;
+        }
+        wait(0.01);
+        posC = Enc->getPulses()/(32*131.25);
+        vel = (posC - posP)/t.read();
+        t.reset();
+        posP = posC;
+        pc.printf("v: %f hm: %d\n\r", MotorPWM, hasNotMoved);
+        if(abs(vel) > 0.001) {
+            hasNotMoved = 0;
+        } else {
+            hasNotMoved++;
+        }
+    } while(hasNotMoved < 6);
+    *E = 0;
+}
+
+void calibrateMotor()
+{
+    moveMotorToStop(&M1, &E1, &Enc1, -0.1);
+    moveMotorToStop(&M2, &E2, &Enc2, 0.2);
+    Enc2.reset();
+    moveMotorToStop(&M1, &E1, &Enc1, -0.1);
+    Enc1.reset();
+    pc.printf("%f", Enc1.getPulses());
+}
+
+void EMG_move()
+{
+
+    double xStart = 0;
+    double yStart = 20;
+
+    double rot1 = calcRot1(xStart, yStart);
+    double rot2 = calcRot2(xStart, yStart);
+
+    moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
+    moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
+    sw2.rise(changespeed);
+    while(true) {
+
+
+        while(emg1_filtered >= 0.4*emg1_cal) {
+            //controlLED = !controlLED ;
+            moveWithSpeed(&xStart, &yStart, 0, speedy);
+            pc.printf("ystart: %g \n\r",yStart);
         }
-        }while(t<10);
-}
\ No newline at end of file
+
+        rot1 = calcRot1(xStart, yStart);
+        rot2 = calcRot2(xStart, yStart);
+        moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
+        moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
+    }
+}
+
+void DEMO_motor()
+{
+    while(1){
+    moveAlongPath(10, 30, -10, 30, 1);
+    moveAlongPath(-10, 30, -10, 20, 1);
+    moveAlongPath(-10, 20, 10, 20, 1);
+    moveAlongPath(10, 20, 10, 30, 1);
+    }
+}
+
+
+
+
+
+void state_machine(void)
+{
+
+    switch(currentState) {
+
+        case WAITING:
+            // Description:
+            // In this state we do nothing, and wait for a command
+
+            // Actions
+            led_red = 0;
+            led_green = 0;
+            led_blue = 0;   // Colouring the led WHITE
+
+            // State transition logic
+            if (button_motor < 1){
+                currentState = MOTOR_ANGLE_CLBRT;
+                stateChanged = true;
+                pc.printf("Starting Calibration\n\r");
+            } else if (Fail_button < 1){
+                currentState = FAILURE_MODE;
+                stateChanged = true;
+            }
+            break;
+
+        case MOTOR_ANGLE_CLBRT:
+            // Description:
+            // In this state the robot moves to a mechanical limit of motion,
+            // in order to calibrate the motors.
+            led_red = 1;
+            led_green = 0;
+            led_blue = 0;
+            timer.start();
+            if (stateChanged) {
+                calibrateMotor();                   // Actuate motor 1 and 2.
+                stateChanged = true;                // Keep this loop going, until the transition conditions are satisfied.
+            }
+            time_in_state = timer.read();
+            if(time_in_state>5) {
+                currentState = EMG_CLBRT ;
+                stateChanged = true;
+                pc.printf("Starting Calibration\n\r");
+            }
+
+            break;
+
+        case EMG_CLBRT:
+            led_red = 1;
+            led_green = 1;
+            led_blue = 0;
+            timer.start() ;
+            if (stateChanged) {
+                EMGcalibration();
+                stateChanged = true;
+            }
+            time_in_state = timer.read();
+            if (time_in_state >=5) {
+                currentState = WAITING4SIGNAL ;
+                stateChanged = true;
+                pc.printf("Waiting for signal");
+                break;
+
+            case WAITING4SIGNAL:
+            led_red = 1;
+            led_green = 0;
+            led_blue = 1;
+                if (button_motor < 1) {
+                    currentState = MOTOR_ANGLE_CLBRT;
+                    stateChanged = true;
+                    pc.printf("Starting Calibration \n\r");
+                } else if (button_demo < 1) {
+                    currentState = MOVE_W_DEMO;
+                    stateChanged = true;
+                    pc.printf("DEMO mode \r\n");
+                    wait(1.0f);
+                } else if (button_emg <1) {
+                    currentState = MOVE_W_EMG;
+                    stateChanged = true;
+                    pc.printf("EMG mode\r\n");
+                    wait(1.0f);
+                } else if (Fail_button == 0) {
+                    currentState = FAILURE_MODE;
+                    stateChanged = true;
+                }
+
+                break;
+
+            case MOVE_W_EMG:
+                if (stateChanged) {
+                    void EMG_move();
+                    stateChanged = true;
+                }
+                if(button_demo < 1) {
+                    currentState = WAITING4SIGNAL ;
+                    stateChanged = true;
+                }
+
+                break;
+
+            case MOVE_W_DEMO:
+                if (stateChanged) {
+                    DEMO_motor() ;
+                    stateChanged = true;
+                }
+                if(button_demo < 1) {
+                    currentState = WAITING4SIGNAL ;
+                    stateChanged = true;
+                }
+
+                break;
+
+            case FAILURE_MODE:
+                if (stateChanged) {
+                    led_red = 0;
+                    led_green = 1;
+                    led_blue = 1;
+                    stateChanged = true;
+                    }
+                break;
+
+
+
+
+}
+}
+}