Diff: main.cpp

Revision:

1:ace33633653b

Parent:

0:43160ef59f9f

Child:

2:94b5e00288a5

--- a/main.cpp	Fri Oct 14 14:18:14 2016 +0000
+++ b/main.cpp	Fri Oct 14 14:35:09 2016 +0000
@@ -18,7 +18,7 @@
 
 //set settings
 Serial pc(USBTX,USBRX);
-Ticker MeasureTicker, BiQuadTicker, FeedbackTickere;// sampleT, TimeTracker;
+Ticker MeasurePTicker; //, BiQuadTicker, FeedbackTickere;// sampleT, TimeTracker;
 HIDScope    scope(2);
 
 //set datatypes
@@ -45,10 +45,10 @@
 BiQuad bq2(1.0000,   -1.5704,    1.2756,   -0.4844,    0.0762);
 
 //set go-Ticker settings
-volatile bool MeasurePTicker_go=false, PTicker_go=false, MotorControllerTicker_go=false;// TimeTracker_go=false, sampleT_go=false;
-void MeasurePTicker_act(){MeasurePTicker_go=true;}; // Activates go-flags
-void PTicker_act(){PTicker_go=true;};
-void MotorControllerTicker_act(){MotorControllerTicker_go=true;};
+volatile bool MeasurePTicker_go=false; //PTicker_go=false, MotorControllerTicker_go=false;// TimeTracker_go=false, sampleT_go=false;
+void MeasureP_act(){MeasurePTicker_go=true;}; // Activates go-flags
+//void P_act(){PTicker_go=true;};
+//void MotorController_act(){MotorControllerTicker_go=true;};
 //void TimeTracker_act(){TimeTracker_go=true;};
 //void sampleT_act(){sampleT_go=true;};
 
@@ -139,24 +139,21 @@
     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);
+        motor1MagnitudePin.write(P_output);
         }
     if(rotation<0){
         motor1DirectionPin.write(ccw);
-        motor1MagnitudePin.write(-output);
+        motor1MagnitudePin.write(-(P_output));
         }
+    pc.printf("ref_position: %d rad/s \r\n", ref_position);
+    pc.printf("position: %d rad \r\n", position);
+    pc.printf("rotation: %d rad \r\n", rotation);
+    pc.printf("Kp: %d \r\n", Kp);
     }
 
 int main()
@@ -164,10 +161,10 @@
  //Initialize
  led1=0;
  led2=0;
- float Potmeter = potMeterIn.read();
- MeasureP.attach(&MeasureP_act, 0.01f); 
- P.attach(&P_act, 0.01f);
- MotorController.attach(&MotorController_act, 0.01f);
+ //float Potmeter = potMeterIn.read();
+ MeasurePTicker.attach(&MeasureP_act, 0.01f); 
+ //P.attach(&P_act, 0.01f);
+ //MotorController.attach(&MotorController_act, 0.01f);
  pc.baud(115200);  
  QEI Encoder(D12, D13, NC, 32); // turns on encoder
  //sampleT.attach(&sampleT_act, 0.1f);
@@ -176,9 +173,10 @@
  
  while(1)
     {
-        if (MeasureP_go){
-            MeasureP_go=false;
+        if (MeasurePTicker_go){
+            MeasurePTicker_go=false;
             MeasureP();
+        }
             // Encoder part
             /*
             counts = Encoder.getPulses();  // gives position
@@ -191,17 +189,18 @@
             countsPrev = counts;
             //pc.printf("Counts: %i rad/s \r\n", counts);
             //pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts);
-            */
+            
         }
         
         if (PTicker_go){
             PTicker_go=false;
             P();
         }
+        
         if (MotorControllerTicker_go){
             MotorControllerTicker_go=false;
             MotorController();
-        /*if (TimeTracker_go){
+        if (TimeTracker_go){
             TimeTracker_go=false;
             TimeTrackerF();
         }