Revision 3:a73502236647, committed 2015-10-21

Comitter:

Zeekat

Date:

Wed Oct 21 12:27:45 2015 +0000

Parent:

2:867a1eb33bbe

Child:

4:c371fc59749e

Commit message:

x,y werkend. hardware doet kut

Changed in this revision

--- a/main.cpp	Tue Oct 20 07:56:11 2015 +0000
+++ b/main.cpp	Wed Oct 21 12:27:45 2015 +0000
@@ -4,6 +4,7 @@
 #include "HIDScope.h"
 #include "math.h"
 
+AnalogOut setpoint_analog(DAC0_OUT);
 Serial pc(USBTX,USBRX);
 HIDScope scope(4);          // definieerd het aantal kanalen van de scope
 
@@ -29,9 +30,6 @@
 Encoder motor1_enc(D12,D11);        // encoder outputpins
 Encoder motor2_enc(D10,D9);
 
-int reference1 = 0;         // set the reference position of the encoders (not used)
-int reference2 = 0;
-
 ////////
 // physical constants
 const double L = 36;           // lenght of arms
@@ -48,8 +46,8 @@
 
 // EXTRA INPUTS AND REQUIRED VARIABLES
 //POTMETERS
-    AnalogIn potright(A0);      // define the potmeter outputpins
-    AnalogIn potleft(A1);
+    AnalogIn potright(A4);      // define the potmeter outputpins
+    AnalogIn potleft(A5);
     
 // Analoge input signalen defineren
 AnalogIn    EMG_in(A2);                 // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen
@@ -69,7 +67,8 @@
         // init values
         bool reverse_links = false;
         bool reverse_rechts = false;
-
+        
+        
 // LED
     DigitalOut ledred(LED1);
     DigitalOut ledgreen(LED2);
@@ -83,13 +82,20 @@
  // Define storage variables for reference values
     double c_reference1 = 0;
     double c_reference2 = 0;
+    
+// define start up variables
+    double start_up_time = 2;
+    double start_loops = start_up_time*controlfreq;
+    int rc1 = 0;
+    int rc2 = 0;
+    
 // limit  angles (in radians)
     // motor1
-    const double limlow1 = 0.5;             // min height
-    const double limhigh1 = 4.5;            // max height
+    const double limlow1 = 1;             // min height in rad
+    const double limhigh1 = 6.3;            // max height in rad
     // motor 2
-    const double limlow2 = -4.5;            // maximum height, motor has been inverted due to transmission
-    const double limhigh2 = 2;              // minimum height
+    const double limlow2 = -4.0;            // maximum height, motor has been inverted due to transmission
+    const double limhigh2 = 2.5;              // minimum height in rad
     
 // Define the maximum rate of change for the reference (velocity)
     double Vmax = 3;            // rad/sec
@@ -111,6 +117,10 @@
     double m2_err_int = 0;
     double m2_prev_err = 0;
 
+// EMG calibration variables
+double emg_gain1 = 0;
+double emg_gain2 = 0;
+double cal_samples = 25;
 
 //// FILTER VARIABLES
 // storage variables
@@ -258,6 +268,30 @@
 double output = P + I + D;
 return output;
 }
+
+void calibrate_amp()
+{
+    double total1 = 0;
+    for(int i = 0; i<cal_samples; i++)
+    {
+        double input1 = EMG_in.read();
+        total1 = total1 + input1;
+        wait(0.1);
+    }
+    emg_gain1 = 1/(total1/cal_samples);
+   // pc.printf("gain1 = %f",emg_gain1);
+    
+    double total2 = 0;
+    for(int i = 0; i<cal_samples; i++)
+    {
+        double input2 = EMG_int.read();
+        total2 = total2 + input2;
+        wait(0.1);
+    }
+    emg_gain2 = 1/(total2/cal_samples);
+   // pc.printf("gain2 = %f",emg_gain2);
+
+}
 /////////////////////////////////////////////////////////////////////
 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
 ///////////////////////////////////////////////////////////////////
@@ -266,25 +300,20 @@
 // MOTOR 1
 void motor1_control()
 {
-    //double m_input1 = potright.read(); // used for pot input
-    //input1 = 0.4505;
-    
-    // first input edit (limit signal between threshold and 1, and reverse if wanted
-    //if(m_input1 < input_threshold) {m_input1 = 0;}
-    //if(m_input1 > 1) {m_input1 = 1;}
-    //if(reverse_rechts == true) {m_input1 = -m_input1;}
-   // m_input1 = biquadfilter(m_input1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);        //biquad with diff-filter  coefficients to smooth input
-    
+       
+    double reference1 = phi_one;  
     
-    //double reference1 = reference_f(m_input1, c_reference1,limlow1,limhigh1);      // determine the reference that has been set by the inputsignal
-    //scope.set(0,reference1);
-    double reference1 = phi_one;
-    scope.set(0,reference1);
-    pc.printf("ref1 = %f",reference1);
+    if(rc1 < start_loops)
+    {
+        rc1++;
+        reference1 = ((double) rc1/start_loops)*reference1;
+    }
+    else
+    {
+        reference1 = reference1;
+    }
+        // m_input1 = biquadfilter(m_input1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);        //biquad with diff-filter  coefficients to smooth input
     double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
-    //scope.set(1,rads1);
-    pc.printf("rads1 = %f ",rads1);
-    //scope.send();
     double error1 = (reference1 - rads1);                       // determine the error (reference - position)
     double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
     
@@ -301,22 +330,19 @@
 // MOTOR 2
 void motor2_control()
 {
-    //double m_input2 = potleft.read()*signalamp2;      // replace potleft with filter
-    
-    // first input limiter
-    //if(m_input2 < input_threshold) {m_input2 = 0;}
-    //if(m_input2 > 1) {m_input2 = 1;}
-    //if(reverse_links == false) {m_input2 = -m_input2;}
-    //m_input2 = biquadfilter(m_input2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
+    double reference2 = phi_two;  
     
-    
-    //double reference2 = reference_f(m_input2, c_reference2,limlow2,limhigh2);      // determine the reference that has been set by the clamped inputsignal
-    
-    double reference2 = phi_two;
-    scope.set(1,reference2);
-    scope.send();
+    if(rc2 < start_loops)
+    {
+        rc2++;
+        reference2 = ((double) rc2/start_loops)*reference2;
+    }
+    else
+    {
+        reference2 = reference2;
+    }    
+
     double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
-    pc.printf("rads2 = %f ",rads2);
     double error2 = (reference2 - rads2);                       // determine the error (reference - position)
     double m_output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
     
@@ -343,8 +369,7 @@
     double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
     output1 = y5;    // update global variable
 // versturen van het input signaal u1 en het gefilterde signaal y5 naar HIDScope channel 0 en 1
-    //scope.set(0,u1);
-    //scope.set(1,y5);
+
 // filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
     double u1t = EMG_int.read();
     double y1t = biquadfiltert( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
@@ -353,29 +378,28 @@
     double y4t = abs(y3t);
     double y5t = biquadfiltert( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
     output2 = y5t; // update global variable
-    
-    // versturen van input signaal2; u1t en het gefilterde signaal; y5t naar HIDScope channel 2 en 3
-    //scope.set(2,u1t);
-    //scope.set(3,y5t);
-    // verzenden van de verstuurde signalen
-    //scope.send();
  }
  
  // function that updates the required motor angles
  void det_angles()
 {
+    // potmeter debugging
+    // static double time = 0.0;
+    // const double time_inc = 0.005;
+    // setpoint_analog= 0.5+(sin(time)/2.0);
+    // time += time_inc;    
+    
     output1 = potright.read();
+
     output2 = potleft.read();
-    double xx = 66+output1*4;
-    double yy = -16+output2*32;
+    double xx = 50+output1*20;
+    
+    double ymin = - 16;
+    double ymax = + 16;
+    double yy = ymin+output2*(ymax-ymin);
     double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
     double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
     double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
-    // hoeken berekenen
-    //pc.printf("xx = %f ",xx);
-    //pc.printf("yy = %f    ",yy);
-    //scope.set(0,xx);
-    //scope.set(1,yy);
     
     double theta_one = (atan2(yy,xx)+beta); 
     double theta_two = (-pi + alfa); 
@@ -383,29 +407,26 @@
     double phi1 = 4*(theta_one) + 2.8;
     double phi2 = 4*(theta_one+theta_two) + 1.85;
     phi2 = -phi2;
+    
+    scope.set(0,output1);
+    scope.set(1,output2);
+    scope.send();
+    
     if(phi1 < limlow1){phi1 = limlow1;}
     if(phi1 > limhigh1){phi1 = limhigh1;}
     
-    if(phi2 < limlow2){phi2 = limlow2;}           // reverse limlow2 limhigh2 becasue they are inverse due to transmission 
+    if(phi2 < limlow2){phi2 = limlow2;}           
     if(phi2 > limhigh2){phi2 = limhigh2;}
     
+    phi1 = biquadfilter(phi1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
+    phi2 = biquadfilter(phi2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
+        
     phi_one = phi1;
     phi_two = phi2;
     
-    //theta_one = theta_one*4 ;
-    //theta_two = theta_two*4 ;
-    
+    //pc.printf("x = %f, y = %f, phi_one = %f, phi_two = %f \n",xx,yy,phi_one,phi_two);
 
     
-    //pc.printf("theta1 = %f ",theta_one);
-    //pc.printf("theta2 = %f \n",theta_two);
-    //scope.set(2,phi1);
-    //scope.set(3,phi2);
-    
-    //scope.send();
-    // testen van de functie
-    double xt = L*cos(theta_one)+L*cos(theta_one+theta_two);
-    double yt = L*sin(theta_one)+L*sin(theta_one+theta_two);
 
 }
 //////////////////////////////////////////////////////////////////
@@ -466,6 +487,7 @@
             wait(reverse_button_rechts.read() == 1);
             wait(0.3);             
         }
+
         //////////////////////////////////////////////////
         // Main Control stuff and options
         if(loop_start == true && calib_start == false)        // check if start button = true then start the main control loops
@@ -480,10 +502,22 @@
         if(loop_start == false) {motor1_aan.write(0); motor2_aan.write(0);}
         
         // turn green led on // start calibration procedures
-        if(loop_start == false && calib_start == true) { LED(1,0,1); motor1_aan.write(0); motor2_aan.write(0);}
+        if(loop_start == false && calib_start == true) 
+        {   LED(1,0,1); 
+            motor1_aan.write(0); 
+            motor2_aan.write(0);
+            calibrate_amp();        // 10 second calibration
+            calib_start = false;    // turn fork off
+        }
         
         // turn red led on
-        if(loop_start == true && calib_start == true) { LED(0,1,1); motor1_aan.write(0); motor2_aan.write(0);}
+        if(loop_start == true && calib_start == true) 
+        { 
+        LED(0,1,1); 
+        motor1_aan.write(0); 
+        motor2_aan.write(0);
+        // switch_type = !switch_type;
+        }
         
         // turn leds off (both buttons false)
         else { LED(1,1,1);}