Diff: main.cpp

Revision:

53:35282a3e0cad

Parent:

52:fd35025574ca

Child:

54:a14acf0d1683

--- a/main.cpp	Thu Oct 31 20:47:57 2019 +0000
+++ b/main.cpp	Thu Oct 31 21:11:16 2019 +0000
@@ -36,6 +36,9 @@
 AnalogIn emg3_in (A2); // Third muscle -> TBD
 AnalogIn emg4_in (A3); // Third muscle -> TBD
 
+// Analog Potmeter inputs
+AnalogIn potmeter1 (A4);
+AnalogIn potmeter2 (A5);
 
 // Encoder inputs
 DigitalIn encA1(D9);
@@ -51,7 +54,6 @@
 
 // Servo
 Servo myservo(D3);
-bool butt1;
 
 /*
 ------------------------------ INITIALIZE TICKERS, TIMERS & TIMEOUTS ------------------------------
@@ -103,6 +105,7 @@
 bool button1_pressed = false;
 bool button2_pressed = false;
 bool switch2_pressed = false;
+bool servo_button1;
 
 // LED states
 enum LED_Colors { off, red, green, blue, purple, yellow, cyan, white }; // Define possible LED colors
@@ -730,15 +733,15 @@
 
     if ( q1 < -5.0f * deg2rad) {
         q1 = -5.0f * deg2rad;
-    } else if ( q1 > 65.0f*deg2rad ) {
+    } else if ( q1 > 65.0f * deg2rad ) {
         q1 = 65.0f * deg2rad;
     } else {
         q1 = q1;
     }
 
-    if ( q2 > -50.0f*deg2rad ) {
+    if ( q2 > -50.0f * deg2rad ) {
         q2 = -50.0f * deg2rad;
-    } else if ( q2 < -138.0f*deg2rad ) {
+    } else if ( q2 < -138.0f * deg2rad ) {
         q2 = -138.0f * deg2rad;
     } else {
         q2 = q2;
@@ -780,8 +783,8 @@
 
 void changeservo()
 {
-    butt1= extButton1.read();
-    if (butt1==1) {
+    servo_button1= extButton1.read();
+    if (servo_button1 == 1) {
         myservo.SetPosition(2000);
     } else {
         myservo.SetPosition(1000);
@@ -877,8 +880,8 @@
     // Do stuff until end condition is met
     EMGOperationFunc();
 
-    Vx = emg1_out * 15.0f * emg1_dir;
-    Vy = emg2_out * 15.0f * emg2_dir;
+    Vx = emg1_out * (10.0 + 10.0 * potmeter1.read() ) * emg1_dir;
+    Vy = emg2_out * (10.0 + 10.0 * potmeter2.read() ) * emg2_dir;
 
     PID_controller();
     motorControl();
@@ -918,8 +921,8 @@
     // Do stuff until end condition is met
     EMGOperationFunc();
 
-    Vx = emg1_out * 15.0f * emg1_dir;
-    Vy = emg2_out * 15.0f * emg2_dir;
+    Vx = emg1_out * (10.0 + 10.0 * potmeter1.read() ) * emg1_dir;
+    Vy = emg2_out * (10.0 + 10.0 * potmeter2.read() ) * emg2_dir;
 
     PID_controller();
     motorControl();
@@ -1405,10 +1408,11 @@
 
     while(true) {
         pc.printf("Global state: %i EMG state: %i Motor state: %i Operation state: %i Demo state: %i\r\n", global_curr_state, emg_curr_state, motor_curr_state, operation_curr_state, demo_curr_state);
+        pc.printf("Potmeter 1: %f Potmeter 2: %f\r\n", potmeter1.read(), potmeter2.read());
         //pc.printf("EMG1 direction: %f EMG2 direction: %f \r\n", emg1_dir, emg2_dir);
-        pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg);
-        pc.printf("Motor1ref: %f Motor1angle: %f\r\n",motor1_ref*rad2deg/5.5f,motor1_angle*rad2deg/5.5f);
-        pc.printf("Motor2ref: %f Motor2angle: %f\r\n",motor2_ref*rad2deg/2.75f,motor2_angle*rad2deg/2.75f);
+        //pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg);
+        //pc.printf("Motor1ref: %f Motor1angle: %f\r\n",motor1_ref*rad2deg/5.5f,motor1_angle*rad2deg/5.5f);
+        //pc.printf("Motor2ref: %f Motor2angle: %f\r\n",motor2_ref*rad2deg/2.75f,motor2_angle*rad2deg/2.75f);
 
         //pc.printf("Xe: %f Ye: %f\r\n",xe,ye);
         wait(0.5f);