Revision 1:811bb0e318a8, committed 2015-11-26

Comitter:

Tokalic

Date:

Thu Nov 26 07:21:21 2015 +0000

Parent:

0:be99ed42340d

Commit message:

Step test to csv

Changed in this revision

--- a/main.cpp	Fri Sep 10 13:32:59 2010 +0000
+++ b/main.cpp	Thu Nov 26 07:21:21 2015 +0000
@@ -1,82 +1,71 @@
-/**
- * Drive a robot forwards or backwards by using a PID controller to vary
- * the PWM signal to H-bridges connected to the motors to attempt to maintain
- * a constant velocity.
- */
-
-#include "Motor.h"
+#include "PID.h"
 #include "QEI.h"
-#include "PID.h"
-
-Motor leftMotor(p21, p20, p19);  //pwm, inB, inA
-Motor rightMotor(p22, p17, p18); //pwm, inA, inB
-QEI leftQei(p29, p30, NC, 624);  //chanA, chanB, index, ppr
-QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr
-//Tuning parameters calculated from step tests;
-//see http://mbed.org/cookbook/PID for examples.
-PID leftPid(0.4312, 0.1, 0.0, 0.01);  //Kc, Ti, Td
-PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
+#define RATE  0.01
+#define Kc    0.473
+#define Ti    0.025
+#define Td    0.0
 
-int main() {
-
-    leftMotor.period(0.00005);  //Set motor PWM periods to 20KHz.
-    rightMotor.period(0.00005);
+PwmOut Motor(p21);
+QEI Qei(p29, p30, NC, 504);
+PID Controller(Kc, Ti, Td, RATE);
+//-------
+// Datoteke
+//-------
+LocalFileSystem local("local");
+FILE* fp;
 
-    //Input and output limits have been determined
-    //empirically with the specific motors being used.
-    //Change appropriately for different motors.
-    //Input  units: counts per second.
-    //Output units: PwmOut duty cycle as %.
-    //Default limits are for moving forward.
-    leftPid.setInputLimits(0, 3000);
-    leftPid.setOutputLimits(0.0, 0.9);
-    leftPid.setMode(AUTO_MODE);
-    rightPid.setInputLimits(0, 3200);
-    rightPid.setOutputLimits(0.0, 0.9);
-    rightPid.setMode(AUTO_MODE);
+Timer endTimer;
+
+//Radne varijable
 
+volatile int Pulses     = 0;
+volatile int PrevPulses = 0;
+volatile float PwmDuty  = 0.0;
+volatile float Velocity = 0.0;
+//Zadana brzina
+int goal = 21000;
+ 
 
-    int leftPulses      = 0; //How far the left wheel has travelled.
-    int leftPrevPulses  = 0; //The previous reading of how far the left wheel
-    //has travelled.
-    float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per
-    //second.
-    int rightPulses     = 0; //How far the right wheel has travelled.
-    int rightPrevPulses = 0; //The previous reading of how far the right wheel
-    //has travelled.
-    float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
-    //second.
-    int distance     = 6000; //Number of pulses to travel.
-
-    wait(5); //Wait a few seconds before we start moving.
-
-    //Velocity to mantain in pulses per second.
-    leftPid.setSetPoint(1000);
-    rightPid.setSetPoint(975);
-
-    while ((leftPulses < distance) || (rightPulses < distance)) {
-
-        //Get the current pulse count and subtract the previous one to
-        //calculate the current velocity in counts per second.
-        leftPulses = leftQei.getPulses();
-        leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
-        leftPrevPulses = leftPulses;
-        //Use the absolute value of velocity as the PID controller works
-        //in the % (unsigned) domain and will get confused with -ve values.
-        leftPid.setProcessValue(fabs(leftVelocity));
-        leftMotor.speed(leftPid.compute());
-
-        rightPulses = rightQei.getPulses();
-        rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
-        rightPrevPulses = rightPulses;
-        rightPid.setProcessValue(fabs(rightVelocity));
-        rightMotor.speed(rightPid.compute());
-
-        wait(0.01);
-
+void initializeMotors(void);
+void initializePidControllers(void);
+void initializeMotors(void){
+ 
+    Motor.period_us(50);
+    Motor = 1.0;
+}
+ 
+void initializePidControllers(void){
+ 
+    Controller.setInputLimits(0.0, 54500);
+    Controller.setOutputLimits(0.0, 1.0);
+    Controller.setBias(1.0);
+    Controller.setMode(AUTO_MODE);
+}
+ 
+int main() {
+ wait(2);
+    //Inicijalizacija
+    initializeMotors();
+    initializePidControllers();
+ 
+    //Kreiranje datoteke za spremanje rezultata.
+    fp = fopen("/local/pidtest.csv", "w");
+    
+    endTimer.start();
+ 
+    Controller.setSetPoint(goal);
+ 
+    //Vrijeme mjerenja je 3 sekunde
+    while (endTimer.read() < 3.0){
+        Pulses = Qei.getPulses();
+        Velocity = (Pulses - PrevPulses) / RATE;
+        PrevPulses = Pulses;
+        Controller.setProcessValue(Velocity);
+        PwmDuty = Controller.compute();
+        Motor = PwmDuty;
+        fprintf(fp, "%f,%f,\n", Velocity, PwmDuty);
+        wait(RATE);
     }
-
-    leftMotor.brake();
-    rightMotor.brake();
-
+    //Spremanje izmjerenih podataka
+    fclose(fp);
 }