Diff: main.cpp

Revision:

12:c3fd0712f43d

Parent:

9:913a59894698

Child:

13:74f2e8d3e04e

--- a/main.cpp	Thu Oct 31 14:11:14 2019 +0000
+++ b/main.cpp	Thu Oct 31 16:58:17 2019 +0000
@@ -21,16 +21,16 @@
 
 // COMMUNICATION
 MODSERIAL pc(USBTX, USBRX);
-PulseInOut pulsePin(D2);
+//PulseInOut pulsePin(D2);
 
 void moveWithEMG()
 {
-    int i = int(pulsePin.read_high_us(1000) / 100.00 + 0.5);
-    if(i > 0 && i < 10) {
-        int EMG_A = (i > 6) ? 2 : (i > 3) ? 1 : 0;
-        int EMG_B = (i == 1 || i == 4 || i == 7) ? 0 : (i == 2 || i == 5 || i == 8) ? 1 : 2;
-        // Do something here
-    }
+//    int i = int(pulsePin.read_high_us(1000) / 100.00 + 0.5);
+//    if(i > 0 && i < 10) {
+//        int EMG_A = (i > 6) ? 2 : (i > 3) ? 1 : 0;
+//        int EMG_B = (i == 1 || i == 4 || i == 7) ? 0 : (i == 2 || i == 5 || i == 8) ? 1 : 2;
+//        // Do something here
+//    }
 }
 
 struct vec {
@@ -158,7 +158,7 @@
 
 // AIMING //
 float aimAngle, aimTilt;
-float cueLength = 18;
+float cueLength = 15;
 void aim(float angle, float tilt)  // Moves both stepper and servo so the end affector points towards desired angle
 {
     setServo(tilt + 90);
@@ -211,10 +211,6 @@
     encoder1.reset(); // Reset encoder positions
     encoder2.reset();
     encoder3.reset();
-
-    //setServo(0);
-    setSolenoid(0);
-    //setLaser(0);
 }
 
 
@@ -222,8 +218,8 @@
 {
     pc.baud(115200);
     pc.printf("\r\nStarting...\r\n\r\n");
-    motor1_pwm.period(0.0001);
-    motor3_pwm.period(0.0001);
+    motor1_pwm.period(0.020);
+    motor3_pwm.period(0.020);
     stepper_moveToAngle.attach(&stepper_move, 0.0015);
 
     while (true) {
@@ -232,6 +228,8 @@
                 calibrate();
                 currentState = PHASE_1;
                 setServo(90);
+                setSolenoid(0);
+                setLaser(1);
                 break;
             case PHASE_1:
                 if(demo) moveWithJoystick();
@@ -246,8 +244,9 @@
                     currentState = PHASE_2;
                     wait_ms(1000);
                     ballPos.x = endPos.x;
-                    ballPos.y = 65;
+                    ballPos.y = 75;
                     ballPos.z = endPos.z;
+                    setLaser(0);
                 }
                 wait_ms(40);
                 break;
@@ -274,6 +273,10 @@
                 getMotorPositions(); // Calculate current positions
                 moveToTargets(); // Set the motors speeds accordingly
                 wait_ms(40);
+                
+                if(joyButton.read() == 0){
+                    setSolenoid(1);
+                }
                 break;
         }
         /*