Diff: main.cpp

Revision:

4:060478ea7fbd

Parent:

3:8caef4872b0c

--- a/main.cpp	Mon Oct 17 10:39:56 2016 +0000
+++ b/main.cpp	Mon Oct 17 13:11:41 2016 +0000
@@ -19,7 +19,7 @@
 //set settings
 Serial pc(USBTX,USBRX);
 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
-//HIDScope    scope(2);
+HIDScope    scope(2);
 
 //set datatypes
 int counts = 0;
@@ -48,9 +48,10 @@
 
 //define encoder counts and degrees
 QEI Encoder(D12, D13, NC, 32); // turns on encoder
-const int counts_per_revolution = 4200;
-const int gear_ratio = 131;
-const float resolution = counts_per_revolution/(2*PI/gear_ratio);  //counts per radian
+const int counts_per_revolution = 4200; //counts per motor axis revolution
+const int inverse_gear_ratio = 131;
+//const float motor_axial_resolution = counts_per_revolution/(2*PI);
+const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio);  //87567.0496892 counts per radian, encoder axis
 
 float GetReferencePosition()
 {
@@ -61,21 +62,23 @@
     float referencePosition = Potmeter1 * maxPosition; //Potmeter1 * maxPosition; //refpos in radians 
     pc.printf("Max Position: %f rad \r\n", maxPosition);
     //pc.printf("Potmeter1, refpos: %f \r\n", Potmeter1);
-    pc.printf("Ref Position: %f rad \r\n", referencePosition);
+    pc.printf("Motor Axis Ref Position: %f rad \r\n", referencePosition);
     return referencePosition;
 }
 
 float FeedForwardControl(float referencePosition)
 {
-    //QEI Encoder(D13, D12, NC, 32); // turns on encoder
-    //int counts = Encoder.getPulses();           // gives position of encoder 
-    float Position = counts/resolution;         //position in radians
+    float EncoderPosition = counts/resolution;         //position in radians, encoder axis
+    float Position = EncoderPosition*inverse_gear_ratio;        //position in radians, motor axis
     // linear feedback control
     float ControllerInput = referencePosition - Position; // 'error' in radians
+    scope.set(0,referencePosition);
+    scope.set(1,Position);
+    scope.send();
     float Kp = potMeter2.read(); //Potmeter2;
     float motorValue = ControllerInput * Kp;
-    pc.printf("Position: %f rad \r\n", Position);
-    pc.printf("Counts: %i rad \r\n", counts);
+    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);
     pc.printf("MotorValue: %f \r\n", motorValue);
     return motorValue;
@@ -110,63 +113,14 @@
     SetMotor1(motorValue);
 }
 
-void TimeTrackerF(){
-     //wait(1);   
-     //float Potmeter1 = potMeter1.read();
-     float referencePosition = GetReferencePosition();
-     pc.printf("TTReference Position: %d rad \r\n", referencePosition);
-     //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
-     //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
-     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
@@ -174,15 +128,8 @@
  led2=1;
  pc.baud(115200);
  pc.printf("Test putty");
- //float Potmeter = potMeterIn.read();
- MeasureTicker.attach(&MeasureTicker_act, 1.0f); 
- 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);
+ MeasureTicker.attach(&MeasureTicker_act, 0.01f); 
+ bqc.add(&bq1).add(&bq2); QEI Encoder(D12, D13, NC, 32); // turns on encoder
  
  while(1)
     {
@@ -192,37 +139,6 @@
             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