main.cpp

00001 #include "mbed.h"
00002 #include <math.h>
00003 #include "MODSERIAL.h"
00004 #include "QEI.h"
00005 #include "HIDScope.h"
00006 #include "BiQuad.h"
00007 
00008 /*
00009 THINGS TO CONSIDER
00010 - Line 234, 239: motor action of motor 1 is inverted because it is mounted
00011 opposite to motor 2 in the tower. Check if the clockwise directions of the 
00012 motors correspond to the positive q1, q2-directions (both counterclockwise)
00013  in the original IK-sketch.
00014 - Line 244,257: motor values have been scaled down for safety at first test, restore
00015 after testing to get proper action.
00016 - Set angle boundaries!!
00017 - Set robot constants (lengths etc.)
00018 */
00019 
00020 //set pins
00021 DigitalIn encoder1A (D13); //Channel A van Encoder 1
00022 DigitalIn encoder1B (D12); //Channel B van Encoder 1
00023 DigitalIn encoder2A (D11); //Channel A van Encoder 2, kan niet op D15
00024 DigitalIn encoder2B (D10); //Channel B van Encoder 2, kan niet op D14
00025 //DigitalOut led1 (D11); 
00026 //DigitalOut led2 (D10);
00027 AnalogIn potMeter1(A2);
00028 AnalogIn potMeter2(A1);
00029 DigitalOut motor1DirectionPin(D7);
00030 PwmOut motor1MagnitudePin(D6);
00031 DigitalOut motor2DirectionPin(D4);
00032 PwmOut motor2MagnitudePin(D5);
00033 DigitalIn button1(D3);
00034 DigitalIn button2(D9);
00035 
00036 //library settings
00037 Serial pc(USBTX,USBRX);
00038 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
00039 //HIDScope    scope(4);
00040 
00041 //set initial conditions
00042 float error1_prev = 0;
00043 float error2_prev = 0;
00044 float IntError1 = 0;
00045 float IntError2 = 0;
00046 float q1 = 0;
00047 float q2 = 0;
00048 //set initial conditions for function references
00049  float q1_dot = 0.0;
00050  float q2_dot = 0.0;
00051  float motorValue1 = 0.0;
00052  float motorValue2 = 0.0;
00053  
00054 //set constant or variable values
00055 int counts1 = 0;
00056 int counts2 = 0;
00057 int counts1Prev = 0;
00058 int counts2Prev = 0;
00059 double DerivativeCounts;
00060 float x0 = 1.0;
00061 float L0 = 1.0;
00062 float L1 = 1.0;
00063 float dx;
00064 float dy;
00065 float dy_stampdown = 0.05; //5 cm movement downward to stamp
00066 
00067 float t_sample = 0.01; //seconds
00068 float referenceVelocity = 0;
00069 float bqcDerivativeCounts = 0;
00070 const float PI = 3.141592653589793;
00071 const int cw = 0;       //values for cw and ccw are inverted!! cw=0 and ccw=1
00072 const int ccw = 1;
00073 
00074 //set BiQuad
00075 BiQuadChain bqc;
00076 BiQuad bq1(0.0186,    0.0743,    0.1114,    0.0743,    0.0186); //get numbers from butter filter MATLAB
00077 BiQuad bq2(1.0000,   -1.5704,    1.2756,   -0.4844,    0.0762);
00078 
00079 //set go-Ticker settings
00080 volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
00081 void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
00082 void BiQuadTicker_act(){BiQuadTicker_go=true;};
00083 void FeedbackTicker_act(){FeedbackTicker_go=true;};
00084 void TimeTracker_act(){TimeTracker_go=true;};
00085 //void sampleT_act(){sampleT_go=true;};
00086 
00087 //define encoder counts and degrees
00088 QEI Encoder1(D12, D13, NC, 32); // turns on encoder
00089 QEI Encoder2(D10, D11, NC, 32); // turns on encoder
00090 
00091 const int counts_per_revolution = 4200; //counts per motor axis revolution
00092 const int inverse_gear_ratio = 131;
00093 //const float motor_axial_resolution = counts_per_revolution/(2*PI);
00094 const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio);  //87567.0496892 counts per radian, encoder axis
00095 
00096 void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_dotOut, float &q2_dotOut){
00097     
00098     //get joint positions q from encoder
00099     float Encoder1Position = counts1/resolution;         //position in radians, encoder axis
00100     float Encoder2Position = counts2/resolution;
00101     
00102     float Motor1Position = Encoder1Position*inverse_gear_ratio;        //position in radians, motor axis
00103     float Motor2Position = Encoder2Position*inverse_gear_ratio;
00104       
00105     //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG  
00106     float biceps1 = !button1.read();
00107     float biceps2 = !button2.read();
00108     if (biceps1 > 0 && biceps2 > 0){
00109         //both arms activated: stamp moves down
00110         //led1 = 1;
00111         //led2 = 1;
00112         dx = 0;
00113         dy = 1; //dy_stampdown; //into stamping vertical position?? ~the stamp down action
00114         q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
00115         q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
00116 
00117         /*
00118          wait(1);
00119         dy = -(dy_stampdown);  //reset vertical position
00120         q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
00121         q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
00122         */
00123         }
00124     else if (biceps1 > 0 && biceps2 <= 0){
00125         //arm 1 activated, move left
00126         //led1 = 1;
00127         //led2 = 0;
00128         dx = 1; //-biceps1;
00129         dy = 0;
00130         q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
00131         q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
00132         }
00133     else if (biceps1 <= 0 && biceps2 > 0){
00134         //arm 1 activated, move left
00135         //led1 = 0;
00136         //led2 = 1;
00137         dx = 1; //biceps2;
00138         dy = 0;
00139         q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
00140         q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
00141 
00142         }
00143     else{
00144         //led1 = 0;
00145         //led2 = 0;
00146         dx=0;
00147         dy=0;
00148         q1_dotOut = 0;
00149         q2_dotOut = 0;
00150         }
00151             
00152     //get joint angles change q_dot = Jpseudo * TwistEndEff  (Matlab)
00153     
00154     //update joint angles
00155     q1Out = q1Out + q1_dotOut;  //in radians
00156     q2Out = q2Out + q2_dotOut;
00157     
00158     pc.baud(115200);
00159     pc.printf("dx:          %f \r\n", dx);
00160     pc.printf("dy:          %f \r\n", dy);
00161     pc.printf("q1:          %f \r\n", q1Out);
00162     pc.printf("q1_dot:      %f \r\n", q1_dotOut);
00163     pc.printf("q2:          %f \r\n", q2Out);
00164     pc.printf("q2_dot:      %f \r\n", q2_dotOut);
00165     
00166     pc.printf("Counts1:     %f \r\n", counts1);
00167     pc.printf("Encoder1:    %f \r\n", Encoder1Position);
00168     pc.printf("Motor1:      %f \r\n", Motor1Position);
00169     pc.printf("Counts2:    %f \r\n", counts2);
00170     pc.printf("Encoder2:    %f \r\n", Encoder2Position);
00171     pc.printf("Motor2:      %f \r\n", Motor2Position);
00172     }
00173 
00174 void FeedForwardControl1(float q1_dot, float q2_dot, float &motorValue1Out, float &motorValue2Out){
00175     //float Encoder1Position = counts1/resolution;         //position in radians, encoder axis
00176     //float Position1 = Encoder1Position*inverse_gear_ratio;        //position in radians, motor axis
00177     
00178     // linear feedback control
00179     float error1 = q1_dot; //referencePosition1 - Position1;             // proportional error in radians
00180     float error2 = q2_dot; //referencePosition1 - Position1;             // proportional error in radians
00181     float Kp = 1; //potMeter2.read();
00182 
00183     float IntError1 = IntError1 + error1*t_sample;             // integrated error in radians
00184     float IntError2 = IntError2 + error2*t_sample;             // integrated error in radians
00185     //float maxKi = 0.2;
00186     float Ki = 0.1; //potMeter2.read();
00187     
00188     float DerivativeError1 = (error1_prev + error1)/t_sample;  // derivative of error in radians
00189     float DerivativeError2 = (error2_prev + error2)/t_sample;  // derivative of error in radians
00190     //float maxKd = 0.2;
00191     float Kd = 0.0; //potMeter2.read();
00192     
00193     //scope.set(0,referencePosition1);
00194     //scope.set(1,Position1);
00195     //scope.set(2,Ki);
00196     //scope.send();
00197     
00198     motorValue1Out = error1 * Kp + IntError1 * Ki + DerivativeError1 * Kd;    //total controller output = motor input
00199     motorValue2Out = error2 * Kp + IntError2 * Ki + DerivativeError2 * Kd;    //total controller output = motor input
00200     //pc.printf("Motor Axis Position: %f rad \r\n", Position1);
00201     //pc.printf("Counts encoder1: %i rad \r\n", counts1);
00202     //pc.printf("Kp: %f \r\n", Kp);
00203     //pc.printf("MotorValue: %f \r\n", motorValue1);
00204     
00205     pc.printf("error1: %f \r\n", error1);
00206     pc.printf("IntError1: %f \r\n", IntError1);
00207     pc.printf("DerError1: %f \r\n", DerivativeError1);
00208     pc.printf("error2: %f \r\n", error2);
00209     pc.printf("IntError2: %f \r\n", IntError2);
00210     pc.printf("DerError2: %f \r\n", DerivativeError2);
00211         
00212     error1_prev = error1;
00213     error2_prev = error1;
00214     float biceps1 = !button1.read();
00215     float biceps2 = !button2.read();
00216        
00217     /*
00218     scope.set(0,q1);
00219     scope.set(1,q2);
00220     scope.set(2,biceps1);
00221     scope.set(3,biceps2);
00222     scope.send();
00223     */
00224 }
00225 
00226 void SetMotor1(float motorValue1, float motorValue2)
00227 {
00228     // Given -1<=motorValue<=1, this sets the PWM and direction
00229     // bits for motor 1. Positive value makes motor rotating
00230     // clockwise. motorValues outside range are truncated to
00231     // within range
00232     //control motor 1
00233     if (motorValue1 >=0) //clockwise rotation
00234         {motor1DirectionPin=ccw;        //inverted due to opposite (to other motor) build-up in tower
00235         //led1=1;
00236         //led2=0;
00237         }
00238     else    //counterclockwise rotation 
00239         {motor1DirectionPin=cw;         //inverted due to opposite (to other motor) build-up in tower
00240         //led1=0;
00241         //led2=1;
00242         }
00243     if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
00244         else motor1MagnitudePin = 0.1*fabs(motorValue1);  //fabs(motorValue1);
00245     //control motor 2
00246     if (motorValue2 >=0)  //clockwise rotation
00247         {motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted)
00248         //led1=1;
00249         //led2=0;
00250         }
00251     else    //counterclockwise rotation 
00252         {motor2DirectionPin=cw;    //action is ccw, due to faulty motor2DirectionPin (inverted)
00253         //led1=0;
00254         //led2=1;
00255         }
00256     if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
00257         else motor2MagnitudePin = 0.1*fabs(motorValue2);  //fabs(motorValue1);
00258 }
00259 
00260 void MeasureAndControl()
00261 {
00262     // This function measures the EMG of both arms, calculates via IK what
00263     // the joint speeds should be, and controls the motor with 
00264     // a Feedforward controller. This is called from a Ticker.
00265     GetReferenceKinematics1(q1, q2, q1_dot, q2_dot);
00266     FeedForwardControl1( q1_dot, q2_dot, motorValue1, motorValue2);
00267     SetMotor1(motorValue1, motorValue2);
00268 }
00269 
00270 void TimeTrackerF(){
00271      //wait(1);   
00272      //float Potmeter1 = potMeter1.read();
00273      //float referencePosition1 = GetReferencePosition();
00274      //pc.printf("TTReference Position: %d rad \r\n", referencePosition1);
00275      //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
00276      //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
00277      //pc.printf("TTCounts: %i \r\n", counts1);
00278 }
00279 
00280 /*
00281 void BiQuadFilter(){            //this function creates a BiQuad filter for the DerivativeCounts
00282     //double in=DerivativeCounts();
00283     bqcDerivativeCounts=bqc.step(DerivativeCounts);
00284     //return(bqcDerivativeCounts);
00285     }
00286 */  
00287 
00288 int main()
00289 {
00290  //Initialize
00291  //int led1val = led1.read();
00292  //int led2val = led2.read();
00293   pc.baud(115200);
00294  pc.printf("Test putty IK");
00295  MeasureTicker.attach(&MeasureTicker_act, 1.0f); 
00296  bqc.add(&bq1).add(&bq2);
00297  
00298  while(1)
00299     {
00300         if (MeasureTicker_go){
00301             MeasureTicker_go=false;
00302             MeasureAndControl();
00303             counts1 = Encoder1.getPulses();           // gives position of encoder 
00304             counts2 = Encoder2.getPulses();           // gives position of encoder 
00305             }
00306 /*
00307         if (BiQuadTicker_go){
00308             BiQuadTicker_go=false;
00309             BiQuadFilter();
00310         }
00311   */  
00312     }
00313 }