Diff: main.cpp

Revision:

2:5cace74299e7

Parent:

1:4cb9af313c26

Child:

3:be922ea2415f

--- a/main.cpp	Fri Oct 26 09:12:26 2018 +0000
+++ b/main.cpp	Tue Oct 30 09:15:02 2018 +0000
@@ -17,12 +17,27 @@
 DigitalOut led1(LED1); // Red led
 DigitalOut led2(LED2); // Green led
 DigitalOut led3(LED3); // Blue led
+QEI encoder1(D14, D15, NC, 8400, QEI::X4_ENCODING);
+//QEI encoder2(A2, A3), NC, 4200);
+//QEI encoder3(A4, A5), NC, 4200);
+AnalogIn pot(A0); // Speed knob
 
 bool stateChanged = true;
 
 Ticker mainTicker;
 Timer stateTimer;
-double sampleTime = 0.01;
+
+const double sampleTime = 0.001;
+const float maxVelocity=8.4; // in rad/s
+const double PI = 3.141592653589793238463;
+                                    
+double u1, u2, u3, u4; // u1 is motor output of the long link, u2 is motor of the short link, u3 is motor of gripper, u4 is motor of screwer
+FastPWM pwmpin1(D5); //motor pwm
+DigitalOut directionpin1(D4); // motor direction
+double robotEndPoint;
+
+double v;
+double Dpulses;
 
 void switchToFailureState ()
 {
@@ -35,6 +50,56 @@
     stateChanged = true;
 }
 
+double measureVelocity (int motor)
+{
+    static double lastPulses = 0;
+    double currentPulses;
+    static double velocity = 0;
+    
+    static int i = 0;
+    if (i == 10) { // Encoder is not accurate enough, so with 1000 Hz the velocity can only be 0, 1000, 2000 or 3000 pulses/s
+    switch (motor) { // Check which motor to measure
+        case 1:
+            currentPulses = encoder1.getPulses();
+            break;
+        case 2:
+            //currentPulses = encoder2.getPulses();
+            break;
+        case 3:
+            //currentPulses = encoder3.getPulses();
+            break;
+    }
+
+    double deltaPulses = currentPulses - lastPulses;
+    Dpulses = deltaPulses;
+    velocity = deltaPulses / (sampleTime * 10); // Velocity in pulses/s
+    lastPulses = currentPulses;
+    i = 0;
+    } else {
+        i += 1;
+    }
+    v = velocity;
+    return velocity;
+}
+
+void measureAll ()
+{
+
+}
+
+void getMotorControlSignal ()   // Milestone 1 code, not relevant anymore
+{
+    double potSignal = pot.read(); // read pot and scale to motor control signal
+    //pc.printf("motor control signal = %f\n", potSignal);
+    u1 = potSignal;
+}
+
+void controlMotor ()   // Control direction and speed
+{
+    directionpin1 = u1 > 0.0f; // Either true or false
+    pwmpin1 = fabs(u1);
+}
+
 void stateMachine ()
 {
     switch (currentState) {
@@ -44,11 +109,14 @@
                 led2 = 1;
                 led3 = 1;
                 // Entry action: all the things you do once in this state
-                // Set output motor to 0
+                u1 = 0; // Turn off all motors
+                u2 = 0;
+                u3 = 0;
+                u4 = 0;
                 stateChanged = false;
             }
 
-            if (startButton.read() == false) {
+            if (startButton.read() == false) { // When button is pressed, value is false
                 //pc.printf("Switching to motor calibration");
                 led1 = 1;
                 currentState = MotorCalState;
@@ -61,17 +129,24 @@
                 // Entry action: all the things you do once in this state
                 led2 = 0;
                 // Set motorpwm to 'low' value
+                u1 = 0.6; //TODO: Check if direction is right
+                u2 = 0.6;
                 stateTimer.reset();
                 stateTimer.start();
                 stateChanged = false;
             }
 
             // Add stuff you do every loop
+            getMotorControlSignal();
 
-            if (stateTimer >= 3.0f) { //TODO: add && motor velocity < 0.1f
+            if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && screwingSwitch.read() == false) { //TODO: add && fabs(measureVelocity(2)) < 0.1f 
+                //TODO: Add reset of encoder2
                 led2 = 1;
+                encoder1.reset(); // Reset encoder for the 0 position
                 currentState = EMGCalState;
                 stateChanged = true;
+                u1 = 0; // Turn off motors
+                u2 = 0;
             }
             break;
         case EMGCalState:
@@ -132,7 +207,7 @@
             if (stateChanged == true) {
                 // Entry action: all the things you do once in this state;
                 led2 = 0;
-                led3 = 0; // Emits x together
+                led3 = 0; // Emits light blue together
                 stateChanged = false;
             }
 
@@ -162,13 +237,13 @@
                 led3 = 0; // Emits pink together
                 stateChanged = false;
             }
-            
+
             if (screwDirection == true) {
                 // Screw
             } else {
                 // Unscrew
             }
-            
+
             if (startButton.read() == false) {
                 led1 = 1;
                 led3 = 1;
@@ -179,7 +254,10 @@
         case FailureState:
             if (stateChanged == true) {
                 // Entry action: all the things you do once in this state
-                //TODO: Turn all motors off
+                u1 = 0; // Turn off all motors
+                u2 = 0;
+                u3 = 0;
+                u4 = 0;
                 stateChanged = false;
             }
 
@@ -197,17 +275,24 @@
 void mainLoop ()
 {
     // Add measure, motor controller and output function
+    measureAll();
     stateMachine();
+    controlMotor();
 }
 
 int main()
 {
+    pc.printf("checkpoint 1\n");
     pc.baud(115200);
     mainTicker.attach(mainLoop, sampleTime);
     failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated
 
     while (true) {
-        pc.printf("State = %i\n", currentState);
+        //pc.printf("State = %i\n", currentState);
+        //int pulses = encoder1.getPulses();
+        //pc.printf("pulses = %i\n", pulses);
+        pc.printf("v = %f\n", v);
+        pc.printf("delta pulses = %f\n", Dpulses);
         wait(1);
     }
 }
\ No newline at end of file