Diff: main.cpp

Revision:

8:42d1d02673ae

Parent:

7:05495acc08b0

Child:

9:2435c48d2032

--- a/main.cpp	Wed Nov 01 14:59:42 2017 +0000
+++ b/main.cpp	Thu Nov 02 10:33:03 2017 +0000
@@ -12,8 +12,8 @@
 AnalogIn potMeter2(A1);     //Analoge input of potmeter 2 (will be use for te reference position)
 PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
 DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control
+Encoder motor1(D13,D12,true);
 
-Encoder motor1(D13,D12,true);
 MODSERIAL pc(USBTX,USBRX);
 
 float PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
@@ -32,11 +32,12 @@
 float e_prev2 = 0; 
 float e_int2 = 0;
 
-
+float Potmeterwaarde;
+float potmeterwaarde2;
 
 float GetReferencePosition() 
 {
-    float Potmeterwaarde = potMeter2.read();
+     Potmeterwaarde = potMeter2.read();
     int maxwaarde = 4096;                   // = 64x64
     float refP = Potmeterwaarde*maxwaarde;
     return refP;                            // value between 0 and 4096 
@@ -44,7 +45,7 @@
 
 float GetReferencePosition2() 
 {
-    float potmeterwaarde2 = potmeter1.read();
+     potmeterwaarde2 = potmeter1.read();
     int maxwaarde2 = 4096;                   // = 64x64
     float refP2 = potmeterwaarde2*maxwaarde2;
     return refP2;                            // value between 0 and 4096 
@@ -151,31 +152,33 @@
     float error = (refP - Huidigepositie);// make an error
     float motorValue = FeedBackControl(error, e_prev, e_int);
     SetMotor1(motorValue);
-}
 
-void MeasureAndControl2(void)
-{
     // hier the control of the control system
     float refP2 = GetReferencePosition2(); 
     float Huidigepositie2 = Encoder2(); 
     float error2 = (refP2 - Huidigepositie2);// make an error
     float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
     SetMotor2(motorValue2);
+    
+    pc.baud(115200);
+    pc.printf("potmeter = %f, refP = %f, motorvalue = %f \r\n, potmeter2 = %f, refP2 = %f, motorvalue2 = %f \r\n", Potmeterwaarde, refP, motorValue, potmeterwaarde2, refP2, motorValue2);
 }
 
 
 int main()
 {
     M1E.period(PwmPeriod);
+    M2E.period(PwmPeriod2);
     Treecko.attach(MeasureAndControl, Ts);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
                                             //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
-    DubbelTreecko.attach(MeasureAndControl2, Ts);
+    //DubbelTreecko.attach(MeasureAndControl2, Ts);
      
      
     while(1) 
     {
         wait(0.2);
-        pc.baud(115200);
+        //pc.printf("Potmeter2 = %f, refP = %f, motorValue = %f,  \r\nPotmeter1 = %f, refP2 = %f, motorValue2 = %f \r\n", Potmeterwaarde, refP, motorValue, potmeterwaarde2, refP2, motorValue2);
+        //pc.baud(115200);
         float B = motor1.getPosition();
         float Potmeterwaarde = potMeter2.read();
         //float positie = B%4096;