Diff: main.cpp

Revision:

6:6f26efe946af

Parent:

5:37e230689418

--- a/main.cpp	Mon Oct 17 13:10:20 2016 +0000
+++ b/main.cpp	Mon Oct 17 14:30:02 2016 +0000
@@ -19,7 +19,7 @@
 //set settings
 Serial pc(USBTX,USBRX);
 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
-HIDScope    scope(2);
+HIDScope    scope(6);
 
 //set datatypes
 int counts = 0;
@@ -76,21 +76,27 @@
     float EncoderPosition = counts/resolution;         //position in radians, encoder axis
     float Position = EncoderPosition*inverse_gear_ratio;        //position in radians, motor axis
     // linear feedback control
-    scope.set(0,referencePosition);
-    scope.set(1,Position);
-    scope.send();
-    float error = referencePosition - Position; // 'error' in radians
+    
+    float error = referencePosition - Position;             // proportional error in radians
     float Kp = 1; //potMeter2.read();
     
-    float IntError = IntError + error*t_sample;
-    float maxKi = 0.2;
-    float Ki = potMeter2.read()*maxKi;
+    float IntError = IntError + error*t_sample;             // integrated error in radians
+    //float maxKi = 0.2;
+    float Ki = 0.1; //potMeter2.read();
+    
+    float DerivativeError = (error - error_prev)/t_sample;  // derivative of error in radians
+    //float maxKd = 0.2;
+    float Kd = potMeter2.read()*10;
     
-    //float DerivativeError = (error_prev + error)/t_sample;
-    float maxKd = 0.2;
-    //float Kd = potMeter2.read()*maxKd;
+    scope.set(0,referencePosition);
+    scope.set(1,Position);
+    scope.set(2,error);
+    scope.set(3,error_prev);
+    scope.set(4,DerivativeError);
+    scope.set(5,Kd);
+    scope.send();
     
-    float motorValue = error * Kp + IntError * Ki; //+ DerivativeError * Kd;
+    float motorValue = (error * Kp) + (IntError * Ki) + (DerivativeError * Kd);    //total controller output = motor input
     pc.printf("Motor Axis Position: %f rad \r\n", Position);
     pc.printf("Counts encoder: %i rad \r\n", counts);
     pc.printf("Kp: %f \r\n", Kp);
@@ -139,53 +145,13 @@
      pc.printf("TTCounts: %i \r\n", counts);
 }
 /*
-void sample()
-{
-    int countsPrev = 0;
-    QEI Encoder(D12, D13, NC, 32);
-    counts = Encoder.getPulses();  // gives position
-    //scope.set(0,counts);
-    DerivativeCounts = (counts-countsPrev)/0.001;
-    //scope.set(1,DerivativeCounts);
-    countsPrev = counts; 
-    //scope.send();
-    pc.printf("Counts: %i rad/s \r\n", counts);
-    pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts);
-}
-
 void BiQuadFilter(){            //this function creates a BiQuad filter for the DerivativeCounts
     //double in=DerivativeCounts();
     bqcDerivativeCounts=bqc.step(DerivativeCounts);
     //return(bqcDerivativeCounts);
     }
-    
-void MeasureP(){
-    double ref_position = Potmeter1;                //reference position from potmeter
-    int counts = Encoder.getPulses();           // gives position    
-    double position = counts/resolution;         //position in radians
-    double rotation = ref_position-position;     //rotation is 'position error' in radians
-    double movement = rotation/(2*PI);  //movement in rotations
-    double Kp = Potmeter2;
-    }
-    
-double P(double rotation, double Kp){
-    double P_output = Kp*movement;
-    return P_output;
-    }
-    
-void MotorController(){
-    double output = P(rotation, Kp);
-    
-    if(rotation>0){
-        motor1DirectionPin.write(cw);
-        motor1MagnitudePin.write(output);
-        }
-    if(rotation<0){
-        motor1DirectionPin.write(ccw);
-        motor1MagnitudePin.write(-output);
-        }
-    }
-*/
+  */  
+
 int main()
 {
  //Initialize
@@ -193,15 +159,9 @@
  led2=1;
  pc.baud(115200);
  pc.printf("Test putty");
- //float Potmeter = potMeterIn.read();
  MeasureTicker.attach(&MeasureTicker_act, 0.01f); 
  bqc.add(&bq1).add(&bq2);
- //BiQuadTicker.attach(&BiQuadTicker_act, 0.01f); //frequentie van 100 Hz
- //TimeTracker.attach(&TimeTracker_act, 1.0f);
  QEI Encoder(D12, D13, NC, 32); // turns on encoder
- //sampleT.attach(&sampleT_act, 0.1f);
- //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
- //pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
  
  while(1)
     {
@@ -211,37 +171,11 @@
             counts = Encoder.getPulses();           // gives position of encoder 
             pc.printf("Resolution: %f pulses/rad \r\n",resolution);
             }
-      /*
-            // Encoder part
-            counts = Encoder.getPulses();  // gives position
-            DerivativeCounts = ((double) counts-countsPrev)/0.01;  
-            
-            scope.set(0,counts);
-            scope.set(1,DerivativeCounts);
-            //scope.set(1,bqcDerivativeCounts);
-            scope.send();
-            countsPrev = counts;
-            //pc.printf("Counts: %i rad/s \r\n", counts);
-            //pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts);
-        }
-        
+/*
         if (BiQuadTicker_go){
             BiQuadTicker_go=false;
             BiQuadFilter();
         }
-    
-        if (FeedbackTicker_go){
-            FeedbackTicker_go=false;
-            Feedback();
-        
-        if (TimeTracker_go){
-            TimeTracker_go=false;
-            TimeTrackerF();
-        }
-        
-        if (sampleT_go){
-            sampleT_go=false;
-            sample();
-        }*/
+*/    
     }
 }
\ No newline at end of file