Revision 1:0f0423207b62, committed 2016-03-22

Comitter:

ms523

Date:

Tue Mar 22 10:57:41 2016 +0000

Parent:

0:3a132f85c1a8

Child:

2:be3b6a72f823

Commit message:

working in velocity mode

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Classic_PID.lib	Tue Mar 22 10:57:41 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/Vitec-Group/code/Classic_PID/#7b42de70b65f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PanVelocityLoop.cpp	Tue Mar 22 10:57:41 2016 +0000
@@ -0,0 +1,69 @@
+#include "mbed.h"
+#include "iC_MU.h"
+#include "rtos.h"
+#include "Classic_PID.h"
+
+// Define limits for zero crossing
+// These values should allow operation upto 3750 RPM
+#define bits 18                 // The number of bits we want to use
+#define OneTurn (1<<bits)       // 262144 counts per rev
+#define Lower (1<<(bits-5))     // 8192 counts = 11.25 degrees
+#define Upper OneTurn - Lower   // 262144 - 8192 = 253952
+
+extern iC_MU icMu;
+extern PwmOut Pan_Motor_PWM;
+extern DigitalOut Pan_Motor_Direction;
+extern Classic_PID PanVelocityPID;
+extern Serial pc;
+
+extern bool running;
+extern float amplitude;
+extern float a;
+extern float b;
+
+int LasticMuPosition = 0;
+int run_count = 0;
+
+void PanVelocityLoop(void const *args)
+{
+     float Duty_Cycle = 0.0;
+     
+     // Find the icMu data
+    int icMuPosition = icMu.ReadPOSITION() >> (19 - bits);          // Read the current position from the iC-MU and bitshift to reduce noise
+    int icMuVelocity = icMuPosition - LasticMuPosition;             // Calculate change in position (i.e. Velocity)
+    
+    if(running){
+        pc.printf("\n\r%d %d",run_count,icMuPosition);
+        
+        if(run_count < 5000){
+            PanVelocityPID.setSetPoint(amplitude * sin((a*run_count*run_count)+(b*run_count)));
+            run_count++;        // Inc run_count     
+        } else {
+            running = 0;        // Stop the test
+            PanVelocityPID.setSetPoint(0);
+        }
+    }
+    
+    // Check to see if the icMu has gone past the index point
+    if(icMuPosition < Lower & LasticMuPosition > Upper) {           // The icMu has gone over the index point in 1 direction
+        icMuVelocity += OneTurn;
+    } else if(icMuPosition > Upper & LasticMuPosition < Lower) {    // The icMu has gone over the index point in the other direction
+        icMuVelocity -= OneTurn;
+    }
+    
+    LasticMuPosition = icMuPosition;                                // Update new position from next time
+    
+    // Set the process value
+    PanVelocityPID.setProcessValue(icMuVelocity);                   // Pass the Velocity onto the PID loop
+
+    
+    Duty_Cycle = PanVelocityPID.compute();
+
+    if(Duty_Cycle < 0) {
+        Pan_Motor_Direction = 1;
+        Pan_Motor_PWM = 1 - (Duty_Cycle * -1.0);
+    } else {
+        Pan_Motor_Direction = 0;
+        Pan_Motor_PWM = 1 - Duty_Cycle;
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/iC_MU.lib	Tue Mar 22 10:57:41 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/Vitec-Group/code/iC_MU/#ae562ccce5bd

--- a/main.cpp	Tue Mar 22 08:33:17 2016 +0000
+++ b/main.cpp	Tue Mar 22 10:57:41 2016 +0000
@@ -1,42 +1,81 @@
 #include "mbed.h"
+#include "iC_MU.h"
+#include "rtos.h"
+#include "Classic_PID.h"
+
+// iC-MU Encoder Objects
+iC_MU icMu(p5,p6,p7,p8);
+
+// Pan Motor
+PwmOut Pan_Motor_PWM(p21);                      // Purple wire
+DigitalOut Pan_Motor_Direction(p22);            // Yellow wire
+
+/* Kp = 0.00018, Ki = 0.000006, Kd = 0.0, 0.0001 */
+Classic_PID PanVelocityPID(0.00018, 0.000006, 0.0, 0.0001);     //Kp, ki, kd, kvelff
 
-DigitalOut myled(LED1);
+// Serial objects
 Serial pc (USBTX,USBRX);
-Timer t;
+
+// Global Variables
+float run_time = 5.0;
+float start_Hz = 1.0, stop_Hz = 2.0;
+float amplitude;
+bool running = 0;
+float w1;
+float w2;
+float a;
+float b;
+
+// External variables
+extern int LasticMuPosition;
+
+// Functions
+void PanVelocityLoop(void const *args);
 
 int main()
 {
-    float run_time = 5.0;
-    float start_Hz = 1.0, stop_Hz = 2.0;
-    float amplitude;
-    
+    // Set up the Pan motor
+    Pan_Motor_PWM.period_us(50);                // Set PWM to 20 kHz
+    Pan_Motor_PWM = 1;                          // Start with motor static
+
+    // Initalise Pan Velocity loop RtosTimer thread
+    RtosTimer PanVelocityLoop_timer(PanVelocityLoop, osTimerPeriodic);
+    PanVelocityLoop_timer.start(1);             // Run at 1kHz
 
+    // Initalise Pan PID Loop
+    PanVelocityPID.setProcessLimits(1.0, -1.0);
+    PanVelocityPID.setSetPoint(0);
+    LasticMuPosition = icMu.ReadPOSITION() >> 1;
+
+    // Increase terminal speed
+    pc.baud(921600);
+
+    // Prompt the user to enter the test parameters
     pc.printf("\n\r Enter Start frequency (Hz): ");
     pc.scanf("%f", &start_Hz);
     pc.printf("%f",start_Hz);
-    
+
     pc.printf("\n\r Enter Stop frequency (Hz): ");
     pc.scanf("%f", &stop_Hz);
     pc.printf("%f",stop_Hz);
-    
+
     pc.printf("\n\r Enter Amplitude (encoder counts): ");
     pc.scanf("%f", &amplitude);
     pc.printf("%f",amplitude);
-    
+
     pc.printf("\n\n\r Press any key to start test...");
     pc.getc();
-    
-    float w1 = start_Hz * 3.14159 * 2;
-    float w2 = stop_Hz * 3.14159 * 2; 
-    float a = (w2 - w1) / (2 * run_time); 
-    float b = w1;    
-    float now;
-       
-    t.start();
+
+    w1 = start_Hz * 3.14159 * 2;
+    w2 = stop_Hz * 3.14159 * 2;
+    a = (w2 - w1) / (2 * run_time * 1000000);
+    b = w1 / 1000;
 
-    while(t.read() < run_time) {
-        now = t.read();
-        pc.printf("\n\r%f",amplitude * sin((a*now*now)+(b*now)));
+    // Set the test running
+    running = 1;
+
+    while(1) {
+        ;
     }
 
 }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Tue Mar 22 10:57:41 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#d3d0e710b443