Diff: main.cpp

Revision:

15:47d949e2de1a

Parent:

14:6a82804c88d6

--- a/main.cpp	Fri Nov 01 14:25:34 2019 +0000
+++ b/main.cpp	Sat Nov 02 19:21:44 2019 +0000
@@ -19,15 +19,13 @@
 
 #define PI           3.14159265358979323846
 
-// COMMUNICATION
 MODSERIAL pc(USBTX, USBRX);
-PulseInOut pulsePin(D9);
 
 bool demo = false;
 enum State {CALIBRATE, INIT_0, IDLE, INIT_1, PHASE_1, INIT_2, PHASE_2, INIT_3, PHASE_3, PHASE_1EMG};
 State currentState = CALIBRATE;
 
-struct vec {
+struct vec { // 3D vector(x,y,z) to store positions
     double x,y,z;
 };
 
@@ -35,29 +33,27 @@
 vec ballPos{10,50,10};
 
 // EMG CONTROLS
-DigitalIn EMG_D(D15);
-AnalogIn EMG_A(A5);
+DigitalIn EMG_D(D15); // LDR connected, used to indicate which arm is sending atm, (left arm/right arm)
+AnalogIn EMG_A(A5); // LDR connected, used to indicate what that arm is doing, (idle/move left/move right)
 void moveWithEMG()
 {
-    int whichEMG = EMG_D.read();
+    int whichEMG = EMG_D.read(); // 0 or 1, left arm or right arm
     float EMGvalue = EMG_A.read();
-    int EMGstate = (EMGvalue < 0.29) ? 0 : (EMGvalue < 0.7) ? 1 : 2;
-    float delta = 0.08;
-    pc.printf("whichEMG: %d\r\n",whichEMG);
-    pc.printf("EMGstate: %d\r\n",EMGstate);
+    int EMGstate = (EMGvalue < 0.29) ? 0 : (EMGvalue < 0.7) ? 1 : 2;  // 0 if the led is off, 1 if it's half brightness, 2 if it's on completely
+    float delta = 0.08; // How much to move in the xz direction
     if(whichEMG == 0) {
-        if(EMGstate == 1) endPos.x -= delta;
-        if(EMGstate == 2) endPos.x += delta;
+        if(EMGstate == 1) endPos.x -= delta; // Move the end position a bit to the left
+        if(EMGstate == 2) endPos.x += delta; // ... to the right
     } else {
-        if(EMGstate == 1) endPos.z -= delta;
-        if(EMGstate == 2) endPos.z += delta;
+        if(EMGstate == 1) endPos.z -= delta; // ... away
+        if(EMGstate == 2) endPos.z += delta; // ... towards us
     }
 }
 
 // JOYSTICK CONTROL //
 AnalogIn joyX(A2), joyY(A1), slider(A0);
 DigitalIn joyButton(PTB20);
-void moveWithJoystick()
+void moveWithJoystick() // Move the endpositon based on joystick input.
 {
     float delta = 0.04;
     if(joyX.read() < 0.2) endPos.x += delta;
@@ -76,7 +72,7 @@
 float motor1_cur = 0, motor2_cur = 0, motor3_cur = 0;
 float motor1_tar = 0, motor2_tar = 0, motor3_tar = 0;
 
-void setMotor(int motor, float motor_spd) // Set the motor speed (between -1 and 1)
+void setMotor(int motor, float motor_spd) // Set the motor speed (between -1 and 1) of a motor (1, 2, or 3)
 {
     int motor_dir = (motor_spd >= 0) ? 0 : 1;
     motor_spd = fabs(motor_spd);
@@ -88,7 +84,7 @@
         motor3_dir.write(motor_dir);
         motor3_pwm.write(motor_spd);
     } else if(motor == 2) {
-        motor2_A.write((motor_dir == 0) ? 0 : int(motor_spd)); // Motor 2 has digital pins so no PWM
+        motor2_A.write((motor_dir == 0) ? 0 : int(motor_spd)); // Motor 2 has digital pins so no PWM, always fully on or fully off
         motor2_B.write((motor_dir == 0) ? int(motor_spd) : 0);
     }
 }
@@ -102,30 +98,30 @@
     motor3_cur = float(encoder3.getPulses()) / 41920.00 + 57.8;
 }
 
-void moveToTargets() // Move to the target positions. No PID.
+void moveToTargets() // Move to the target positions. No PID. If the error(the fabs(.....)) is smaller than some threshold, then the motors are off, otherwise they are fully on for motor 2/3, and 0.08 speed for motor 1.
 {
     setMotor(1, (fabs(motor1_cur - motor1_tar) < 0.5) ? 0 : (motor1_cur < motor1_tar) ? 0.08 : -0.08);
     setMotor(2, (fabs(motor2_cur - motor2_tar) < 0.04) ? 0 : (motor2_cur < motor2_tar) ? 1 : -1);
     setMotor(3, (fabs(motor3_cur - motor3_tar) < 0.04) ? 0 : (motor3_cur < motor3_tar) ? 1: -1);
 }
 
-// KINEMATICS //
-void calculateKinematics(float x, float y, float z)  // y is up
-{
-    float angle1 = fmod(2*PI - atan2(z, x), 2*PI) * (360 / (2*PI));
-    float angle2 = acos(sqrt(x*x + y*y + z*z) / 100.00) + atan2(sqrt(x*x + z*z), y);
-    float angle3 = 2 * asin(sqrt(x*x + y*y + z*z) / 100.00);
+// KINEMATICS // (NOT USED ANYMORE, USED THE NEXT FUNCTION, WHICH TAKES THE SERVO POSITION AS ENDPOINT)
+//void calculateKinematics(float x, float y, float z)  // y is up
+//{
+//    float angle1 = fmod(2*PI - atan2(z, x), 2*PI) * (360 / (2*PI));
+//    float angle2 = acos(sqrt(x*x + y*y + z*z) / 100.00) + atan2(sqrt(x*x + z*z), y);
+//    float angle3 = 2 * asin(sqrt(x*x + y*y + z*z) / 100.00);
+//
+//    motor1_tar = angle1;
+//    motor2_tar = sqrt(200 - 200 * cos(angle2)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = c = 10cm)
+//    motor3_tar = sqrt(2600 - 1000 * cos(PI - angle3)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = 10cm, c = 50cm)
+//}
 
-    motor1_tar = angle1;
-    motor2_tar = sqrt(200 - 200 * cos(angle2)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = c = 10cm)
-    motor3_tar = sqrt(2600 - 1000 * cos(PI - angle3)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = 10cm, c = 50cm)
-}
-
-void calculateKinematicsHand(float x, float y, float z)  // y is up
+void calculateKinematicsHand(float x, float y, float z)  // y is up, calculate target positions of the motors using the kinematics
 {
-    float angle1 = fmod(2*PI - atan2(z, x), 2*PI) * (360 / (2*PI));
-    float xz = sqrt(x*x + z*z) - 15;
-    y -= 9;
+    float angle1 = fmod(2*PI - atan2(z, x), 2*PI) * (360 / (2*PI)); // Make sure the angle is between 0-360 degrees
+    float xz = sqrt(x*x + z*z) - 15; // Offset 15cm because servo is 15cm away from endaffector in the 2D plane
+    y -= 9; // Offset y with 9 cm because the servo is 9 cm below endaffector
     float angle2 = acos(sqrt(xz*xz + y*y) / 100.00) + atan2(xz, y);
     float angle3 = 2 * asin(sqrt(xz*xz +y*y) / 100.00);
 
@@ -134,14 +130,14 @@
     motor3_tar = sqrt(2600 - 1000 * cos(PI - angle3)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = 10cm, c = 50cm)
 }
 
-// STEPPER MOTOR //
+// STEPPER MOTOR // 
 DigitalOut STEPPER_IN1(PTB18), STEPPER_IN2(PTB19), STEPPER_IN3(PTC1), STEPPER_IN4(PTC8);
 
 int stepper_steps = 0;
 float stepper_angle = 0, stepper_target = 0;
 
 void stepper_step(int direction_) // Requires ~1.5ms wait time between each step, 4096 steps is one rotation
-{
+{ // Stepper motor has electromagnets inside, depending on which are on/off, the motor will move to a different position, this code does that.
     STEPPER_IN1 = (stepper_steps == 5 || stepper_steps == 6 || stepper_steps == 7);
     STEPPER_IN2 = (stepper_steps == 3 || stepper_steps == 4 || stepper_steps == 5);
     STEPPER_IN3 = (stepper_steps == 1 || stepper_steps == 2 || stepper_steps == 3);
@@ -179,7 +175,7 @@
     endPos.y = handY;
     endPos.z = handZ;
 
-    float stepperAngle = angle - fmod(2*PI - atan2(handZ, handX), 2*PI) * (180 / PI);
+    float stepperAngle = angle - fmod(2*PI - atan2(handZ, handX), 2*PI) * (180 / PI); // Make sure it stays between 0-360 degrees
     if(stepperAngle < 0) stepperAngle += 360;
     stepper_target = stepperAngle;
 }
@@ -188,7 +184,7 @@
 DigitalOut solenoidA(PTC0), solenoidB(PTC9);
 void setSolenoid(int dir) // 1 is out, 0 is in
 {
-    solenoidA = (dir == 0) ? 0 : 1;
+    solenoidA = (dir == 0) ? 0 : 1; // IMPOSSIBLE TO CREATE SHORT CIRCUIT THIS WAY, A is always inverse of B!!
     solenoidB = (dir == 0) ? 1 : 0;
 }
 
@@ -210,7 +206,7 @@
     setMotor(3,1);
 
     while(!(switch1.read() < 0.5 && switch2.read() > 0.5 && switch3.read() > 0.5)) { // When all switches have been pushed in, stop
-        if(switch1.read() < 0.5) setMotor(1,0);
+        if(switch1.read() < 0.5) setMotor(1,0); // When one of the switches has been pushed in, stop that motor
         if(switch2.read() > 0.5) setMotor(2,0);
         if(switch3.read() > 0.5) setMotor(3,0);
         wait_ms(30);
@@ -231,39 +227,22 @@
     motor3_pwm.period(0.020);
     stepper_moveToAngle.attach(&stepper_move, 0.0015);
 
-//    while(true) {
-//        pc.printf("EMG_D: %d\r\n",EMG_D.read());
-//        pc.printf("EMG_A: %f\r\n",EMG_A.read());
-//        wait_ms(100);
-//    }
-
-//    while(true){
-//        for(int i = 0; i<180; i++){
-//            setServo(i);
-//            wait_ms(50);
-//        }
-//    }
-//    while(true){
-//        moveWithEMG();
-//        wait_ms(20);
-//        }
-
     while (true) {
         switch(currentState) {
             case CALIBRATE: // Calibrate the hand
                 setSolenoid(0);
                 calibrate();
                 setServo(90);
-                currentState = INIT_0;
+                currentState = INIT_0; // Go to next state after calibration
                 break;
-            case INIT_0: // Go to idle position
+            case INIT_0: // Go to idle position (x=0, y=55, z=20);
                 calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
                 getMotorPositions(); // Calculate current positions
                 moveToTargets(); // Set the motors speeds accordingly
                 wait_ms(40);
-                if(fabs(motor1_cur - motor1_tar) < 0.6 && fabs(motor2_cur - motor2_tar) < 0.05 && fabs(motor3_cur - motor3_tar) < 0.05) currentState = IDLE;
+                if(fabs(motor1_cur - motor1_tar) < 0.6 && fabs(motor2_cur - motor2_tar) < 0.05 && fabs(motor3_cur - motor3_tar) < 0.05) currentState = IDLE; // if idle position reached, go to next state
                 break;
-            case IDLE:
+            case IDLE: // Wait for button press, stop all motors
                 setMotor(1,0);
                 setMotor(2,0);
                 setMotor(3,0);
@@ -272,10 +251,10 @@
                     if(demo) currentState = PHASE_1;
                     else currentState = PHASE_1EMG;
                     setLaser(1);
-                    wait_ms(1000);
+                    wait_ms(1000); // Wait a second after the button has been pressed so the button check in the next phase doesn't trigger
                 }
                 break;
-            case PHASE_1EMG:
+            case PHASE_1EMG: // Move endaffector with EMG
                 moveWithEMG();
                 setServo(80);
                 calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
@@ -287,7 +266,7 @@
                 }
                 wait_ms(40);
                 break;
-            case PHASE_1:
+            case PHASE_1: // MOVE endaffector with joystick
                 moveWithJoystick();
                 setServo(80);
                 calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
@@ -304,7 +283,7 @@
                 wait_ms(40);
                 break;
 
-            case PHASE_2:
+            case PHASE_2: // Control angle/tilt of the cue with joystick/slider
                 if(slider.read() > 0.9) aimTilt += 0.25;
                 else if(slider.read() < 0.1) aimTilt -= 0.25;
                 aimTilt = (aimTilt < 0) ? 0 : (aimTilt > 90) ? 90 : aimTilt;
@@ -324,7 +303,7 @@
                     currentState = PHASE_3;
                 }
                 break;
-            case PHASE_3: // Shoot
+            case PHASE_3: // Shoot, then reset position and go to idle
                 setSolenoid(1);
                 wait_ms(500);
                 setSolenoid(0);
@@ -354,9 +333,6 @@
         * SERVO
         setServo(...) // value between 0.0 and 180.0 (= 0 and 180 degrees)
 
-        * AIMING
-        aim(...) // value between 0 and 360
-
         * SOLENOID
         setSolenoid(...); // position, 0(in) or 1(out)
 
@@ -364,7 +340,5 @@
         setLaser(...) // 0(off) or 1(on)
 
         */
-
-//        pc.printf("jx: %f\r\n",joyX.read());
     }
 }