Revision 46:9b60a3c1acab, committed 2018-11-01

Comitter:

Spekdon

Date:

Thu Nov 01 20:54:51 2018 +0000

Parent:

45:829a3992d689

Commit message:

Presentation code

Changed in this revision

--- a/help_functions/PID_controller.h	Thu Nov 01 19:48:22 2018 +0000
+++ b/help_functions/PID_controller.h	Thu Nov 01 20:54:51 2018 +0000
@@ -2,7 +2,7 @@
 
 double Kp = 0.1;
 double Ki = 0.4;
-double Kd = 4;
+double Kd = 5;
 extern double samplingfreq = 1000;
 
 //void PID_controller(double error1, double error2, double &u1, double &u2, const double &T)

--- a/main.cpp	Thu Nov 01 19:48:22 2018 +0000
+++ b/main.cpp	Thu Nov 01 20:54:51 2018 +0000
@@ -17,9 +17,9 @@
 AnalogIn    emg1( A1 );
 AnalogIn    emg2( A2 );
 AnalogIn    emg3( A3 );
-//Servo myservo(A5);
+Servo       gripper(A5);
 //InterruptIn button1(SW2);
-//InterruptIn button2(SW3);
+DigitalIn   buttongripper(D1);
 double range;
 
 // motor ouptuts
@@ -49,6 +49,7 @@
 // Timeouts and related variables
 Timeout make_button_active;
 bool button_suppressed = false;
+bool gripperopen = true;
 
 // States
 enum States {failure, waiting, calib_emg, calib_enc, operational, demo, homing}; //All possible robot states
@@ -117,14 +118,6 @@
 
 // Functions
 
-//void open_gripper(){
-//    myservo=0;
-//}
-//
-//void close_gripper(){
-//    myservo=1;
-//}
-
 vector<double> forwardkinematics_function(double& q1, double& q2) {
     // input are joint angles, output are x and y position of end effector
     
@@ -193,12 +186,13 @@
 }
 
 void output_all() {
-    motor1_pwm = fabs(u1);
+    motor1_pwm = 0.4+0.6*fabs(u1);
+    
     if (motor1_pwm > 1.0f){
         motor1_pwm = 1.0f;
     }
     motor1_dir = u1 > 0.0f;
-    motor2_pwm = fabs(u2);
+    motor2_pwm = 0.4+0.6*fabs(u2);
     if (motor2_pwm > 1.0f){
         motor2_pwm = 1.0f;
     }
@@ -428,7 +422,15 @@
                 make_button_active.attach(&unsuppressing_button,0.5);
 
             } 
-         
+            
+            if (buttongripper.read() == false && button_suppressed == false ) {
+                button_suppressed = true;
+                if (gripperopen){gripper = 0; gripperopen = false;}
+                else {gripper = 1; gripperopen = true;}
+
+                make_button_active.attach(&unsuppressing_button,0.5);
+            }
+            
             break;
             
         case demo: //moving according to a specified trajectory
@@ -533,12 +535,14 @@
     led_R = 1;
     led_B = 1;
     led_G = 1;
-//    myservo = 1;
-//    button1.fall(&open_gripper);
-//    button2.fall(&close_gripper);
     
-    while (true) {pc.printf("error1: %f, error2: %f, pwm1: %f, pwm2: %f, qref1: %f, qref2: %f, vx: %f, vy: %f, q1: %f, q2: %f \r\n",  e1, e2, u1, u2, qref1, qref2, des_vx, des_vy, x_norm, y_norm);
-    testconfig = forwardkinematics_function(qref1,qref2);
+    gripper = 1;
+    int m = 0;
+    while (true) {
+    m++;
+    if (m%1000)
+    {   pc.printf("error1: %f, error2: %f, pwm1: %f, pwm2: %f, qref1: %f, qref2: %f, vx: %f, vy: %f, q1: %f, q2: %f, gripper: \r\n",  e1, e2, u1, u2, qref1, qref2, des_vx, des_vy, q1, q2);}
+    //testconfig = forwardkinematics_function(qref1,qref2);
     //double x = testconfig[0];
     //double y = testconfig[1];
     //pc.printf("x: %f, y: %f \r\n", x,y);