Diff: main.cpp

Revision:

2:867a1eb33bbe

Parent:

1:f3910e46b831

Child:

3:a73502236647

--- a/main.cpp	Sun Oct 18 21:36:16 2015 +0000
+++ b/main.cpp	Tue Oct 20 07:56:11 2015 +0000
@@ -3,10 +3,9 @@
 #include "encoder.h"
 #include "HIDScope.h"
 #include "math.h"
-// #include "complex.h"
 
 Serial pc(USBTX,USBRX);
-HIDScope scope(3);          // definieerd het aantal kanalen van de scope
+HIDScope scope(4);          // definieerd het aantal kanalen van de scope
 
 // Define Tickers and control frequencies
 Ticker          controller1, controller2, main_filter, cartesian;        // definieer de ticker die controler1 doet
@@ -43,8 +42,8 @@
 double output1;
 double output2;
 
-double theta_one;
-double theta_two;
+double phi_one;
+double phi_two;
 
 
 // EXTRA INPUTS AND REQUIRED VARIABLES
@@ -102,7 +101,7 @@
     const double m1_Kd = 0.4;         // differentiation constant
     // motor 2
     const double m2_Kp = 2;
-    const double m2_Ki = 0;
+    const double m2_Ki = 0.5;
     const double m2_Kd = 0.1;
 // storage variables
     // motor 1
@@ -267,21 +266,25 @@
 // MOTOR 1
 void motor1_control()
 {
-    double m_input1 = potright.read(); // used for pot input
+    //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
+    //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 = reference_f(m_input1, c_reference1,limlow1,limhigh1);      // determine the reference that has been set by the inputsignal
+    //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);
     double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
-    scope.set(1,rads1);
-    scope.send();
+    //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);
     
@@ -298,17 +301,22 @@
 // MOTOR 2
 void motor2_control()
 {
-    double m_input2 = potleft.read()*signalamp2;      // replace potleft with filter
+    //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);
+    //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 = reference_f(m_input2, c_reference2,limlow2,limhigh2);      // determine the reference that has been set by the clamped inputsignal
+    //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();
     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);
     
@@ -356,23 +364,49 @@
  // function that updates the required motor angles
  void det_angles()
 {
-    double xx = 7*50*output1;
-    double yy = 7*50*output2;
+    output1 = potright.read();
+    output2 = potleft.read();
+    double xx = 66+output1*4;
+    double yy = -16+output2*32;
     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
-    theta_one = atan2(yy,xx)+beta;
-    theta_two = pi + alfa;
+    //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); 
+    
+    double phi1 = 4*(theta_one) + 2.8;
+    double phi2 = 4*(theta_one+theta_two) + 1.85;
+    phi2 = -phi2;
+    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 > limhigh2){phi2 = limhigh2;}
+    
+    phi_one = phi1;
+    phi_two = phi2;
+    
+    //theta_one = theta_one*4 ;
+    //theta_two = theta_two*4 ;
+    
+
+    
+    //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
-    scope.set(0,xx);
-    scope.set(1,yy);
     double xt = L*cos(theta_one)+L*cos(theta_one+theta_two);
     double yt = L*sin(theta_one)+L*sin(theta_one+theta_two);
-    scope.set(2,xt);
-    scope.set(3,yt);
-    scope.send();
+
 }
 //////////////////////////////////////////////////////////////////
 //////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
@@ -399,7 +433,7 @@
 int main()
 {
     pc.baud(115200);
-    main_filter.attach(&EMG_activate, controlstep);
+    // main_filter.attach(&EMG_activate, controlstep);
     cartesian.attach(&angle_activate, controlstep);
     controller1.attach(&motor1_activate, controlstep);      // call a go-flag
     controller2.attach(&motor2_activate, controlstep);