Revision 2:12efc0d812e1, committed 2015-11-25

Comitter:

Tokalic

Date:

Wed Nov 25 10:51:33 2015 +0000

Parent:

1:ac598811dd00

Commit message:

Mbed PID regulator with 1x16 lcd display

Changed in this revision

diff -r ac598811dd00 -r 12efc0d812e1 TextLCD.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Wed Nov 25 10:51:33 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/wim/code/TextLCD/#d3496c3ea301

diff -r ac598811dd00 -r 12efc0d812e1 main.cpp
--- a/main.cpp	Sat Nov 27 11:37:20 2010 +0000
+++ b/main.cpp	Wed Nov 25 10:51:33 2015 +0000
@@ -1,104 +1,113 @@
-//****************************************************************************/
-// Includes
-//****************************************************************************/
 #include "PID.h"
 #include "QEI.h"
+#include "TextLCD.h"
 
-//****************************************************************************/
-// Defines
-//****************************************************************************/
+
+
 #define RATE  0.01
-#define Kc   -2.6
-#define Ti    0.0
+#define Kc    0.473
+#define Ti    0.025
 #define Td    0.0
 
-//****************************************************************************/
-// Globals
-//****************************************************************************/
-//--------
-// Motors 
-//--------
-//Left motor.
-PwmOut leftMotor(p23);
-DigitalOut leftBrake(p19);
-DigitalOut leftDirection(p28);
-QEI leftQei(p29, p30, NC, 624);
-PID leftController(Kc, Ti, Td, RATE);
-//-------
-// Files
-//-------
-LocalFileSystem local("local");
-FILE* fp;
-//--------
-// Timers
-//--------
-Timer endTimer;
-//--------------------
-// Working variables.
-//--------------------
-volatile int leftPulses     = 0;
-volatile int leftPrevPulses = 0;
-volatile float leftPwmDuty  = 1.0;
-volatile float leftVelocity = 0.0;
+TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x2); // rs, e, d4-d7
+// Motor
+PwmOut Motor(p21);
+QEI Qei(p29, p30, NC, 504);
+PID Controller(Kc, Ti, Td, RATE);
+
+// Definiranje konstanti
+
+
+Timer t;                        
+int startTimeUs, endTimeUs;     
+int deltaUs = 0;                
+int startPulse, endPulse;       
+int deltaPulse = 0;             
+int i = 0;
+
+
+
+int Pulses     = 0;
+int PrevPulses = 0;
+float PwmDuty  = 1.0;
+float Velocity = 0.0;
 //Velocity to reach.
-int goal = 3000;
+int goal = 21000;
+
+
 
-//****************************************************************************/
-// Prototypes
-//****************************************************************************/
-//Set motors to go "forward", brake off, not moving.
+// Deklaracija funkcija
+
+//Set motors to go "forward".
 void initializeMotors(void);
 //Set up PID controllers with appropriate limits and biases.
 void initializePidControllers(void);
 
-void initializeMotors(void){
+float calcRpm(float deltaTimeSec, int deltaPulse);          
+
+float calcRpm(float deltaTimeSec, int deltaPulse)           
+{                                                           
+     return 60.0 * ((deltaPulse / deltaTimeSec) / 1008 );   
+}                                                           
 
-    leftMotor.period_us(50);
-    leftMotor = 1.0;
-    leftBrake = 0.0;
-    leftDirection = 0;
+
+void initializeMotors(void)
+{
+
+    Motor.period_us(50);
+    Motor = 1.0;
 
 }
 
-void initializePidControllers(void){
+void initializePidControllers(void)
+{
 
-    leftController.setInputLimits(0.0, 10500.0);
-    leftController.setOutputLimits(0.0, 1.0);
-    leftController.setBias(1.0);
-    leftController.setMode(AUTO_MODE);
+    Controller.setInputLimits(0.0, 54500.0);
+    Controller.setOutputLimits(0.0, 1.0);
+    Controller.setBias(1.0);
+    Controller.setMode(AUTO_MODE);
 
 }
 
-int main() {
+int main()
+{
 
     //Initialization.
     initializeMotors();
     initializePidControllers();
-
-    //Open results file.
-    fp = fopen("/local/pidtest.csv", "w");
-    
-    endTimer.start();
-
-    //Set velocity set point.
-    leftController.setSetPoint(goal);
+//Set velocity set point.
+    Controller.setSetPoint(goal);
 
-    //Run for 3 seconds.
-    while (endTimer.read() < 3.0){
-        leftPulses = leftQei.getPulses();
-        leftVelocity = (leftPulses - leftPrevPulses) / RATE;
-        leftPrevPulses = leftPulses;
-        leftController.setProcessValue(leftVelocity);
-        leftPwmDuty = leftController.compute();
-        leftMotor = leftPwmDuty;
-        fprintf(fp, "%f,%f\n", leftVelocity, goal);
+    startTimeUs = t.read_us();              
+    startPulse = Qei.getPulses();           
+    t.start();                              
+    
+    
+    while (1) {
+        
+        
+        Pulses = Qei.getPulses();
+        Velocity = (Pulses - PrevPulses) / RATE;
+        PrevPulses = Pulses;
+        Controller.setProcessValue(Velocity);
+        PwmDuty = Controller.compute();
+        Motor = PwmDuty;
+        
+        //Ispis brzine na TeraTerm
+        if(i > 30) {
+        endTimeUs = t.read_us();                                    
+        // endPulse = Qei.getPulses();                                
+        deltaUs = endTimeUs - startTimeUs;                          
+        deltaPulse =Pulses - startPulse;
+        lcd.locate(0,0);                         
+        lcd.printf("N = %.0f\n",calcRpm(deltaUs/1000000.0, deltaPulse)); 
+        lcd.printf(" okr/min"); 
+        startPulse = Qei.getPulses();                               
+        startTimeUs = t.read_us();
+        i = 0;                                  
+        }
+        i = i+1;
         wait(RATE);
+        
     }
-
-    //Stop motors.
-    leftMotor  = 1.0;
-    
-    //Close results file.
-    fclose(fp);
-
-}
+}
\ No newline at end of file