Diff: main.cpp

Revision:

7:1906d404cd1e

Parent:

6:e67250b8b100

Child:

8:5896424eb539

--- a/main.cpp	Thu Nov 01 19:35:15 2018 +0000
+++ b/main.cpp	Tue Nov 06 10:32:00 2018 +0000
@@ -5,15 +5,15 @@
 #include <iostream>
 #include <string>
 
-enum States {WaitState, MotorCalState, EMGCalState, HomingState, MovingState, GrippingState, ScrewingState, FailureState};
+enum States {WaitState, MotorCalState, EMGCalState, HomingState, DemoState ,MovingState, GrippingState, ScrewingState, FailureState};
 States currentState = WaitState;
 
 DigitalIn startButton(D0);
 InterruptIn failureButton(D1);
-DigitalIn grippingSwitch(SW2);
-DigitalIn screwingSwitch(SW3);
-DigitalIn gripDirection(D2);
-DigitalIn screwDirection(D3);
+DigitalIn gripperButton(D2);
+DigitalIn directionSwitch(D3);
+DigitalIn gripperMotorButton(D14);
+
 
 Serial pc(USBTX, USBRX);
 
@@ -23,8 +23,6 @@
 QEI encoder1(D10, D11, NC, 8400, QEI::X4_ENCODING);
 QEI encoder2(D12, D13, NC, 8400, QEI::X4_ENCODING);
 //QEI encoder3(A4, A5), NC, 4200);
-AnalogIn pot(A4); // Speed knob
-AnalogIn pot2(A5);
 
 AnalogIn emg0(A0);
 AnalogIn emg1(A1);
@@ -41,22 +39,22 @@
 const double PI = 3.141592653589793238463;
 const double  L1 = 0.328;
 const double L2 = 0.218;
- double T1[3][3] {
+double T1[3][3] {
     {0, -1, 0},
     {1, 0, 0,},
     {0, 0, 0,}
 };
- double  T20[3][3] {
+double  T20[3][3] {
     {0, -1, 0},
     {1, 0, -L1,},
     {0, 0, 0,}
 };
- double  H200[3][3] {
+double  H200[3][3] {
     {1, 0, L1+L2},
     {0, 1, 0,},
     {0, 0, 1,}
 };
- double Pe2 [3][1] {
+double Pe2 [3][1] {
     {0},
     {0},
     {1}
@@ -70,8 +68,13 @@
 DigitalOut directionpin1(D4); // motor direction
 FastPWM pwmpin2 (D6);
 DigitalOut directionpin2 (D7);
-double setPointX = 0.218;
-double setPointY = 0.328;
+FastPWM pwmpin3(A4); //motor pwm
+DigitalOut directionpin3(D8); // motor direction
+FastPWM pwmpin4(A5);
+DigitalOut directionpin4(D9);
+
+double setPointX = 0.3;
+double setPointY = 0.28;
 double qRef1;
 double qRef2;
 double qMeas1;
@@ -86,19 +89,19 @@
 
 double C[5][5];
 
-double Kp1 = 150;
-double Ki1 = 1;
-double Kd1 = 10;
-double Kp2 = 50;
-double Ki2 = 0.1;
-double Kd2 = 10;
+double Kp1 = 10;
+double Ki1 = 0;
+double Kd1 = 2;
+double Kp2 = 10;
+double Ki2 = 0;
+double Kd2 = 2;
 
 const int samplepack = 250; //Amount of data points before one cycle completes
 volatile int counter = 0; //Counts the amount of readings this cycle
 volatile double total[4] = {0,0,0,0}; //Total difference between data points and the calibration value this cycle (x+, x-, y+, y-)
 double refvaluebic = 10; //Minimum total value for the motor to move (biceps)
 double refvaluecalf = 50; //Minimum total value for the motor to move (calfs)
-double incr = 0.00005; //increment
+double incr = 0.00002; //increment
 bool moving[4] = {0,0,0,0}; //x+, x-, y+, y-
 
 BiQuad hpf(0.8635,-1.7269,0.8635,-1.9359,0.9394); //5Hz High pass filter
@@ -117,51 +120,57 @@
     stateChanged = true;
 }
 
+
+
+
+
+
 void measureEMG ()
 {
-    total[0] += abs(bqc.step(emg0.read()));
-    total[1] += abs(bqc.step(emg1.read()));
-    total[2] += abs(bqc.step(emg2.read()));
-    total[3] += abs(bqc.step(emg3.read()));
-    counter++;
-    if (counter >= samplepack){
-        moving[0] = 0;
-        moving[1] = 0;
-        moving[2] = 0;
-        moving[3] = 0;
-        if (total[0] >= refvaluebic){
-            moving[0] = 1;
+    if (currentState == MovingState) {
+        total[0] += abs(bqc.step(emg0.read()));
+        total[1] += abs(bqc.step(emg1.read()));
+        total[2] += abs(bqc.step(emg2.read()));
+        total[3] += abs(bqc.step(emg3.read()));
+        counter++;
+        if (counter >= samplepack) {
+            moving[0] = 0;
+            moving[1] = 0;
+            moving[2] = 0;
+            moving[3] = 0;
+            if (total[0] >= refvaluebic) {
+                moving[0] = 1;
+            }
+            if (total[1] >= refvaluebic) {
+                moving[1] = 1;
+            }
+            if (total[2] >= refvaluecalf) {
+                moving[2] = 1;
+            }
+            if (total[3] >= refvaluecalf) {
+                moving[3] = 1;
+            }
+            pc.printf("Totals of {x+,x-,y+,y-} are %f, %f, %f, %f \r\n",total[0],total[1],total[2],total[3]);
+            pc.printf("Coordinates: (%f,%f)\r\n", setPointX, setPointY);
+            counter = 0; //Reset for next cycle
+            for (int i=0; i<4; i++) { //clear 'total' array
+                total[i] = 0;
+            }
         }
-        if (total[1] >= refvaluebic){
-            moving[1] = 1;
-        }
-        if (total[2] >= refvaluecalf){
-            moving[2] = 1;
+
+        if(moving[0]) {
+            setPointX += incr;
         }
-        if (total[3] >= refvaluecalf){
-            moving[3] = 1;
+        if (moving[1]) {
+            setPointX -= incr;
         }
-        //pc.printf("Totals of {x+,x-,y+,y-} are %f, %f, %f, %f \r\n",total[0],total[1],total[2],total[3]);
-        pc.printf("Coordinates: (%f,%f)\r\n", setPointX, setPointY);
-        counter = 0; //Reset for next cycle
-        for (int i=0;i<4;i++){ //clear 'total' array
-            total[i] = 0;
+        if (moving[2]) {
+            setPointY += incr;
+        }
+        if (moving[3]) {
+            setPointY -= incr;
         }
     }
-
-    if(moving[0]){
-        setPointX += incr;
-    }
-    if (moving[1]){
-        setPointX -= incr;
-    }
-    if (moving[2]){
-        setPointY += incr;
-    }
-    if (moving[3]){
-        setPointY -= incr;
-    }
-   
 }
 
 double measureVelocity (int motor)
@@ -198,19 +207,19 @@
 
 void measurePosition() // Measure actual angle position with the encoder
 {
-     pulses1 = encoder2.getPulses();   // motor 1 is on M2 and ENC2 of biorobotics shield
-     pulses2 = encoder1.getPulses();   // motor 2 is on M1 and ENC1 of biorobotics shield
+    pulses1 = encoder2.getPulses();   // motor 1 is on M2 and ENC2 of biorobotics shield
+    pulses2 = encoder1.getPulses();   // motor 2 is on M1 and ENC1 of biorobotics shield
     qMeas1 = (pulses1) * 2 * PI / 8400  +140.7*PI/180 ; // Calculate the angle relative to the starting point (8400 pulses per revolution) + offset
-    qMeas2 = (pulses2) * 2 * PI / 8400  - 93*PI/180 ;
+    qMeas2 = (pulses2) * 2 * PI / 8400   -87*PI/180;
 
 }
 
 void getMotorControlSignal ()   // Milestone 1 code, not relevant anymore
 {
-    double potSignal = pot.read() * 2 - 1; // read pot and scale to motor control signal
+    //double potSignal = pot.read() * 2 - 1; // read pot and scale to motor control signal
     //pc.printf("motor control signal = %f\n", posampleTimeignal);
-    u1 = potSignal;
-    u2 = potSignal;
+    u1 = 0;
+    u2 = 0;
 }
 
 template<std::size_t N, std::size_t M, std::size_t P>
@@ -239,8 +248,8 @@
 
 void inverseKinematics ()
 {
-    if (currentState == MovingState) {  // Only in the HomingState should the qRef1, qRef2 consistently change
-        
+    if (currentState == MovingState || currentState == DemoState) {  // Only in the HomingState should the qRef1, qRef2 consistently change
+
         double  Pe_set[3][1] {            // defining setpoint location of end effector
             {setPointX},             //setting xcoord to pot 1
             {setPointY},             // setting ycoord to pot 2
@@ -306,7 +315,7 @@
 
 //Determing 'Pulling" force to setpoint
 
-        double kspring= 1;     //virtual stifness of the force
+        double kspring= 0.1;     //virtual stifness of the force
         double Fs[3][1] {                                    //force vector from end effector to setpoint
             {kspring*Pe_set[0][0] - kspring*Pe0[0][0]},
             {kspring*Pe_set[1][0] - kspring*Pe0[1][0]},
@@ -331,7 +340,7 @@
 
 //Calculating joint behaviour
 
-        double b1 =10;
+        double b1 =1;
         double b2 =1;
         //joint friction coefficent
         //double sampleTime = 1/1000;               //Time step to reach the new angle
@@ -356,9 +365,9 @@
 void PID_controller() // Put the error trough PID control to make output 'u'
 
 {
-     if (currentState >= HomingState && currentState < FailureState){
-     // Should only work when we move the robot to a defined position
-         error1 = qRef1 - qMeas1;
+    if (currentState >= HomingState && currentState < FailureState) {
+        // Should only work when we move the robot to a defined position
+        error1 = qRef1 - qMeas1;
         error2 = qRef2 - qMeas2;
 
         static double errorIntegral1 = 0;
@@ -369,7 +378,6 @@
         //Ki = pot2.read() * 0.5; //Only Kd is controlled by a pot, Kp and Ki are constant
 
         // Proportional part:
-        double Kp = pot2.read() * 0.001;
         double u_k1 = Kp1 * error1;
         double u_k2 = Kp2 * error2;
 
@@ -392,8 +400,8 @@
         // Sum all parsampleTime
         u1 = u_k1 + u_i1 + u_d1;
         u2 = u_k2 + u_i2 + u_d2;
-        
-    
+
+
     }
 }
 
@@ -403,6 +411,10 @@
     pwmpin1 = fabs(u1);
     directionpin2 = u2 > 0.0f; // Either true or false
     pwmpin2 = fabs(u2);
+    directionpin3 = u3 > 0.0f; // Either true or false
+    pwmpin3 = fabs(u3);
+    directionpin4 = u4 > 0.0f; // Either true or false
+    pwmpin4 = fabs(u4);
 }
 
 void stateMachine ()
@@ -444,7 +456,7 @@
             // Add stuff you do every loop
             getMotorControlSignal();
 
-            if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && screwingSwitch.read() == false) { //TODO: add && fabs(measureVelocity(2)) < 0.1f
+            if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && startButton.read() == false && fabs(measureVelocity(2)) < 0.1f) { //TODO: add
                 //TODO: Add reset of encoder2
                 led2 = 1;
                 encoder1.reset(); // Reset encoder for the 0 position
@@ -478,13 +490,18 @@
                 led1 = 0;
                 led2 = 0; // EmisampleTime yellow together
                 //TODO: Set qRef1 and qRef2
-                qRef1 = 90 * PI / 180;
-                qRef2 = -qRef1 + 0 *PI/180;
+                qRef1 = 60 * PI / 180;
+                qRef2 = -90 *PI/180;
                 stateChanged = false;
             }
 
             // Nothing extra happens till robot reaches starting position and button is pressed
-
+            if (gripperButton.read() == false) { //TODO: Also add position condition
+                led1 = 1;
+                led2 = 1;
+                currentState = DemoState;
+                stateChanged = true;
+            }
             if (startButton.read() == false) { //TODO: Also add position condition
                 led1 = 1;
                 led2 = 1;
@@ -492,6 +509,31 @@
                 stateChanged = true;
             }
             break;
+        case DemoState:
+            if (stateChanged == true) {
+                stateTimer.reset();
+                setPointX = 0.15;
+                setPointY = 0.3;
+                stateTimer.start();
+                stateChanged = false;
+            }
+            static double blinkTimer = 0;
+            if (blinkTimer >= 0.5) {
+                led2 = !led2;
+                blinkTimer = 0;
+            }
+            blinkTimer += sampleTime;
+            if (stateTimer >= 5.0) {
+                setPointX = -0.1;
+                setPointY = 0.3;
+            }
+            
+            if (stateTimer >= 20.0) {
+                stateTimer.reset();
+                currentState = HomingState;
+                stateChanged = true;
+                }
+            break;
         case MovingState:
             if (stateChanged == true) {
                 // Entry action: all the things you do once in this state;
@@ -501,7 +543,7 @@
                 stateChanged = false;
             }
 
-            if (grippingSwitch.read() == false) {
+            if (gripperButton.read() == false) {
                 led1 = 1;
                 led2 = 1;
                 led3 = 1;
@@ -515,24 +557,35 @@
                 // Entry action: all the things you do once in this state;
                 led2 = 0;
                 led3 = 0; // EmisampleTime light blue together
+                stateTimer.reset();
+                stateTimer.start();
                 stateChanged = false;
             }
 
-            if (gripDirection == true) {
-                // Close gripper
-            } else {
-                // Open gripper
+            if (gripperMotorButton.read() == false) {
+                pc.printf("Button pressed \r\n");
+                u3 = 0.4;
+                if (directionSwitch == true) {
+                    // Close gripper, so positive direction
+                } else {
+                    // Open gripper
+                    u3 = u3 * -1;
+                }
+            } else { // If the button isn't pressed, turn off motor
+                u3 = 0;
             }
 
-            if (screwingSwitch.read() == false) {
+            if (gripperButton.read() == false && stateTimer >= 2.0f) {
                 led2 = 1;
                 led3 = 1;
+                u3 = 0;
                 currentState = ScrewingState;
                 stateChanged = true;
             }
             if (startButton.read() == false) {
                 led2 = 1;
                 led3 = 1;
+                u3 = 0;
                 currentState = MovingState;
                 stateChanged = true;
             }
@@ -545,15 +598,26 @@
                 stateChanged = false;
             }
 
-            if (screwDirection == true) {
-                // Screw
+            if (gripperMotorButton.read() == false) {
+                u4 = 0.3;
+                u3 = -0.35;
+                if (directionSwitch == true) {
+                    // Screw
+                } else {
+                    // Unscrew
+                    u4 = u4 * -1;
+                    u3 = u3 * -1;
+                }
             } else {
-                // Unscrew
+                u4 = 0;
+                u3 = 0;
             }
 
             if (startButton.read() == false) {
                 led1 = 1;
                 led3 = 1;
+                u3 = 0;
+                u4 = 0;
                 currentState = MovingState;
                 stateChanged = true;
             }
@@ -568,12 +632,12 @@
                 stateChanged = false;
             }
 
-            static double blinkTimer = 0;
-            if (blinkTimer >= 0.5) {
+            static double blinkTimer2 = 0;
+            if (blinkTimer2 >= 0.5) {
                 led1 = !led1;
-                blinkTimer = 0;
+                blinkTimer2 = 0;
             }
-            blinkTimer += sampleTime;
+            blinkTimer2 += sampleTime;
 
             break;
     }
@@ -588,7 +652,7 @@
 
 void mainLoop ()
 {
-    // Add measure, motor controller and output function    
+    // Add measure, motor controller and output function
     measureAll();
     stateMachine();
     PID_controller();
@@ -597,8 +661,15 @@
 
 int main()
 {
+    startButton.mode(PullUp);
+    failureButton.mode(PullUp);
+    gripperButton.mode(PullUp);
+    directionSwitch.mode(PullUp);
+    gripperMotorButton.mode(PullUp);
     pc.baud(115200);
     pc.printf("checkpoint 1\n");
+    //pwmpin1.period(sampleTime);
+    //pwmpin2.period(sampleTime);
     bqc.add(&hpf).add(&notch).add(&lpf); //Add bqfilters to bqc
     mainTicker.attach(mainLoop, sampleTime);
     failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated
@@ -607,7 +678,7 @@
         //pc.printf("State = %i\n", currentState);
         //int pulses = encoder1.getPulses();
         //pc.printf("pulses = %i\n", pulses);
-       // pc.printf("v = %f\n", v);
+        // pc.printf("v = %f\n", v);
         pc.printf("Error1 = %f Error2 = %f \n qRef1 = %f rQref2 = %f \r\n qMeas1 = %f qMeas2 = %f \n, Pulses1 = %f Pulses2 = %f \n", error1,error2,qRef1,qRef2,qMeas1,qMeas2, pulses1, pulses2);
         wait(2);
     }