Revision 1:0ef8f077473e, committed 2015-03-04

Comitter:

nicovanduijn

Date:

Wed Mar 04 18:10:47 2015 +0000

Parent:

0:a1e1e939ee6c

Commit message:

Improved comments.; Yet to do: calibrate() function to initialize, more comments, testing

Changed in this revision

--- a/main.cpp	Wed Mar 04 17:54:08 2015 +0000
+++ b/main.cpp	Wed Mar 04 18:10:47 2015 +0000
@@ -19,8 +19,8 @@
 float kd;                                               // Derivative gain
 float ki;                                               // Integrative gain 
 char val;                                               // Needed for Serial communication (need to be global?)
-int timer1=0;                                           // Variable for timer (need to be global?)
-int timer2=0;                                           // Variable 2 for timer (need to be global?)
+int timer1=0;                                           // Variable for timer
+int timer2=0;                                           // Variable 2 for timer
 void drive(float);                                      // Function declaration for driving the motors
 void callback();                                        // Interrupt triggered function for serial communication
 float PID(float);                                       // Function to do PID control
@@ -32,14 +32,13 @@
     uint16_t status = imu.begin();                              // Use the begin() function to initialize the LSM9DS0 library.
     pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);   // Make sure communication is working
     pc.printf("Should be 0x49D4\n\n");
-    float alpha;                                                // Local to hold angle
+    float alpha=0;                                              // Local to hold angle
    // float speed;                                              // Local to hold motor speed
     //float omega;                                              // Local to hold angular velocity
     float desiredAngle=0;                                       // Later set this angle with a setup function
     pc.attach(&callback);                                       // Attach interrupt to serial communication 
     t.start();                                                  // Start the timer
-    
-    
+      
     while(1) {
         timer2=timer1;                                          // Remember the previous timer
         timer1=t.read();                                        // Read the current timer
@@ -77,9 +76,9 @@
 // drive function
 void drive(float speed)
 {
-    int direction;
+    int direction=0;
     if (speed>0)direction=1;
-    if (speed<0) direction=2;
+    if (speed<0)direction=2;
     if(speed==0)direction=0;
    // pc.printf("%f %d\n", speed, direction);
     switch( direction) {                                // depending on what direction was passed
@@ -173,14 +172,14 @@
    
 }
 
-float PID(float error){
-    static float previousError;
-    float integratedError;
-    float derivativeError;
-    float speed;
-    integratedError+=error;
-    derivativeError=error-previousError;
-    speed=ki*integratedError+kp*error+kd*derivativeError;
-    previousError=error;
-    return speed;
+float PID(float error){                                             // Function to calculate speed given the error
+    static float previousError=0;                                     // Static to keep track of previously passed error
+    float integratedError=0;                                          // Local to hold the integrated error
+    float derivativeError=0;                                          // Local for the derivative
+    float speed=0;                                                    // Local for the speed
+    integratedError+=error*(timer1-timer2);                         // Integrate aka. summing up the error*dt
+    derivativeError=(error-previousError)/(timer1-timer2);          // Differentiate aka dy/dt
+    speed=ki*integratedError+kp*error+kd*derivativeError;           // PID control
+    previousError=error;                                            // Remember error for next time
+    return speed;                                                   // Return speed
     }