Diff: main.cpp

Revision:

1:ffa6f4d78c8e

Parent:

0:d02f4c7e8906

Child:

2:625837aa7a56

--- a/main.cpp	Wed Oct 26 08:45:24 2016 +0000
+++ b/main.cpp	Wed Oct 26 10:46:50 2016 +0000
@@ -3,7 +3,7 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 #include "HIDScope.h"  //make sure hidscope cable is also attached
-#include "BiQuad.h"
+
 
 // in gebruik: D(0,1,4,5,6,7,10,11,12,13) 
 
@@ -32,8 +32,8 @@
 const double pi=3.14159265359;
 const double res = 64/(1/131.25*2*pi);     // resolution on gearbox shaft per pulse
 const double V_max=9.0;                    // maximum voltage supplied by trafo
-const double minRadius=0.3;                // minimum radius of arm
-const double maxRadius=0.6;                // maximum radius of arm
+const double minRadius=0.0;                // minimum radius of arm
+const double maxRadius=0.3;                // maximum radius of arm
 const double pulleyDiameter=0.0398;        // pulley diameter
 const double minAngle=-1.25;               // minimum angle for limiting controller
 
@@ -61,30 +61,10 @@
 }
 
  
- void controller(){ //function for executing controller action
-        // convert ref to gearbox angle
-        double theta=atan(refy/refx);
-        double radius=sqrt(pow(refx,2)+pow(refy,2));
-        
-        //limit theta
-        if (theta =< minAngle){
-            theta=minAngle
-        }
-        else if (theta => 0){
-            theta=0;
-        }
-         
-          //limit radius
-        if (radius =< minRadius){
-            radius=minRadius
-        }
-        else if (radius > maxRadius){
-            radius=maxRadius;
-        } 
-        
+ void controller(){ //function for executing controller action              
         //converting radius and theta to gearbox angle    
         ref_angle1=16*theta;    
-        ref_angle2=(-radius+minRadius)/pi/pulleyDiameter;
+        ref_angle2=-radius/pi/pulleyDiameter;
         
                 
         angle1 = Encoder1.getPulses()/res;   //get number of pulses (counterclockwise is positive)
@@ -130,7 +110,7 @@
     motor2.period(0.02f); // period of pwm signal for motor 2
     motor1dir=0; // setting direction to ccw 
     motor2dir=0; // setting direction to ccw
-    control.attach(&controller,m1_Ts); //Ticker for processing encoder input
+    control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
     pc.printf("RESET\n\r");
     while (true) {
         if(controller_go){