Diff: main.cpp

Revision:

4:e7187a17c732

Parent:

3:be922ea2415f

--- a/main.cpp	Wed Oct 31 12:15:34 2018 +0000
+++ b/main.cpp	Thu Nov 01 22:53:01 2018 +0000
@@ -11,10 +11,10 @@
 
 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);
+
 MODSERIAL pc(USBTX, USBRX);
 DigitalOut led1(LED1); // Red led
 DigitalOut led2(LED2); // Green led
@@ -35,22 +35,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}
@@ -60,10 +60,16 @@
 double u2; // 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
 double u3;
 double u4;
+
 FastPWM pwmpin1(D5); //motor pwm
 DigitalOut directionpin1(D4); // motor direction
 FastPWM pwmpin2 (D6);
 DigitalOut directionpin2 (D7);
+FastPWM pwmpin3(A4); //motor pwm
+DigitalOut directionpin3(D8); // motor direction
+FastPWM pwmpin4(A5);
+DigitalOut directionpin4(D9);
+
 double setPointX;
 double setPointY;
 double qRef1;
@@ -427,7 +433,7 @@
                 stateChanged = false;
             }
 
-            if (grippingSwitch.read() == false) {
+            if (gripperButton.read() == false) {
                 led1 = 1;
                 led2 = 1;
                 led3 = 1;
@@ -444,21 +450,35 @@
                 stateChanged = false;
             }
 
-            if (gripDirection == true) {
-                // Close gripper
-            } else {
-                // Open gripper
+            if (gripperMotorButton == false) {
+                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) {
                 led2 = 1;
                 led3 = 1;
+                if (directionSwitch == true) { // If we close the gripper, we also want it keep giving torgue in the next state to hold on to the object
+                    u3 = 0.4;
+                    } else {
+                        u3 = 0;
                 currentState = ScrewingState;
                 stateChanged = true;
             }
             if (startButton.read() == false) {
                 led2 = 1;
                 led3 = 1;
+                if (directionSwitch == true) {
+                    u3 = 0.4;
+                    } else {
+                        u3 = 0;
                 currentState = MovingState;
                 stateChanged = true;
             }
@@ -471,15 +491,26 @@
                 stateChanged = false;
             }
 
-            if (screwDirection == true) {
+            if (gripperMotorButton == false) {
+                u4 = 0.4;
+                u3 = -0.4;
+            if (directionSwitch == true) {
                 // Screw
             } else {
                 // Unscrew
+                u4 = u4 * -1;
+                u3 = u3 * -1;
             }
+            } else {
+                u4 = 0;
+                u3 = 0.4;
+                }
 
             if (startButton.read() == false) {
                 led1 = 1;
                 led3 = 1;
+                u3 = 0;
+                u4 = 0.4;
                 currentState = MovingState;
                 stateChanged = true;
             }
@@ -523,6 +554,11 @@
 int main()
 {
     pc.printf("checkpoint 1\n");
+    startButton.mode(PullUp);
+    failureButton.mode(PullUp);
+    gripperButton.mode(PullUp);
+    directionSwitch.mode(PullUp);
+    gripperMotorButton.mode(PullUp);
     pc.baud(115200);
     mainTicker.attach(mainLoop, sampleTime);
     failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated