running on mbed now
Dependencies: Classic_PID iC_MU mbed-rtos mbed
Diff: main.cpp
- Revision:
- 1:0f0423207b62
- Parent:
- 0:3a132f85c1a8
- Child:
- 2:be3b6a72f823
diff -r 3a132f85c1a8 -r 0f0423207b62 main.cpp --- 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", &litude); 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) { + ; } }