Vitec Group / Mbed 2 deprecated SweptSine

running on mbed now

Dependencies:   Classic_PID iC_MU mbed-rtos mbed

main.cpp@2:be3b6a72f823, 2016-03-22 (annotated)

Committer:
ms523
Date:
Tue Mar 22 15:35:01 2016 +0000
Revision:
2:be3b6a72f823
Parent:
1:0f0423207b62 
working code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ms523 0:3a132f85c1a8 1 #include "mbed.h"
ms523 1:0f0423207b62 2 #include "iC_MU.h"
ms523 1:0f0423207b62 3 #include "rtos.h"
ms523 1:0f0423207b62 4 #include "Classic_PID.h"
ms523 1:0f0423207b62 5
ms523 1:0f0423207b62 6 // iC-MU Encoder Objects
ms523 1:0f0423207b62 7 iC_MU icMu(p5,p6,p7,p8);
ms523 2:be3b6a72f823 8 iC_MU icMu_Output(p5,p6,p7,p11);
ms523 1:0f0423207b62 9
ms523 1:0f0423207b62 10 // Pan Motor
ms523 1:0f0423207b62 11 PwmOut Pan_Motor_PWM(p21); // Purple wire
ms523 1:0f0423207b62 12 DigitalOut Pan_Motor_Direction(p22); // Yellow wire
ms523 1:0f0423207b62 13
ms523 1:0f0423207b62 14 /* Kp = 0.00018, Ki = 0.000006, Kd = 0.0, 0.0001 */
ms523 2:be3b6a72f823 15 //Classic_PID PanVelocityPID(0.00018, 0.000006, 0.0, 0.0); //Kp, ki, kd, kvelff
ms523 2:be3b6a72f823 16 Classic_PID PanVelocityPID(0.000006, 0.0000001, 0.0, 0.0); //Kp, ki, kd, kvelff
ms523 0:3a132f85c1a8 17
ms523 1:0f0423207b62 18 // Serial objects
ms523 0:3a132f85c1a8 19 Serial pc (USBTX,USBRX);
ms523 1:0f0423207b62 20
ms523 1:0f0423207b62 21 // Global Variables
ms523 1:0f0423207b62 22 float run_time = 5.0;
ms523 1:0f0423207b62 23 float start_Hz = 1.0, stop_Hz = 2.0;
ms523 1:0f0423207b62 24 float amplitude;
ms523 1:0f0423207b62 25 bool running = 0;
ms523 1:0f0423207b62 26 float w1;
ms523 1:0f0423207b62 27 float w2;
ms523 1:0f0423207b62 28 float a;
ms523 1:0f0423207b62 29 float b;
ms523 1:0f0423207b62 30
ms523 1:0f0423207b62 31 // External variables
ms523 1:0f0423207b62 32 extern int LasticMuPosition;
ms523 1:0f0423207b62 33
ms523 1:0f0423207b62 34 // Functions
ms523 1:0f0423207b62 35 void PanVelocityLoop(void const *args);
ms523 0:3a132f85c1a8 36
ms523 0:3a132f85c1a8 37 int main()
ms523 0:3a132f85c1a8 38 {
ms523 1:0f0423207b62 39 // Set up the Pan motor
ms523 1:0f0423207b62 40 Pan_Motor_PWM.period_us(50); // Set PWM to 20 kHz
ms523 1:0f0423207b62 41 Pan_Motor_PWM = 1; // Start with motor static
ms523 1:0f0423207b62 42
ms523 1:0f0423207b62 43 // Initalise Pan Velocity loop RtosTimer thread
ms523 1:0f0423207b62 44 RtosTimer PanVelocityLoop_timer(PanVelocityLoop, osTimerPeriodic);
ms523 1:0f0423207b62 45 PanVelocityLoop_timer.start(1); // Run at 1kHz
ms523 0:3a132f85c1a8 46
ms523 1:0f0423207b62 47 // Initalise Pan PID Loop
ms523 1:0f0423207b62 48 PanVelocityPID.setProcessLimits(1.0, -1.0);
ms523 2:be3b6a72f823 49 PanVelocityPID.setSetPoint(262144); // Set the motor to 2^18
ms523 2:be3b6a72f823 50 //PanVelocityPID.setSetPoint(0);
ms523 2:be3b6a72f823 51 //LasticMuPosition = icMu.ReadPOSITION() >> 1;
ms523 2:be3b6a72f823 52
ms523 1:0f0423207b62 53
ms523 1:0f0423207b62 54 // Increase terminal speed
ms523 1:0f0423207b62 55 pc.baud(921600);
ms523 1:0f0423207b62 56
ms523 1:0f0423207b62 57 // Prompt the user to enter the test parameters
ms523 0:3a132f85c1a8 58 pc.printf("\n\r Enter Start frequency (Hz): ");
ms523 0:3a132f85c1a8 59 pc.scanf("%f", &start_Hz);
ms523 0:3a132f85c1a8 60 pc.printf("%f",start_Hz);
ms523 1:0f0423207b62 61
ms523 0:3a132f85c1a8 62 pc.printf("\n\r Enter Stop frequency (Hz): ");
ms523 0:3a132f85c1a8 63 pc.scanf("%f", &stop_Hz);
ms523 0:3a132f85c1a8 64 pc.printf("%f",stop_Hz);
ms523 1:0f0423207b62 65
ms523 0:3a132f85c1a8 66 pc.printf("\n\r Enter Amplitude (encoder counts): ");
ms523 0:3a132f85c1a8 67 pc.scanf("%f", &amplitude);
ms523 0:3a132f85c1a8 68 pc.printf("%f",amplitude);
ms523 1:0f0423207b62 69
ms523 0:3a132f85c1a8 70 pc.printf("\n\n\r Press any key to start test...");
ms523 0:3a132f85c1a8 71 pc.getc();
ms523 1:0f0423207b62 72
ms523 1:0f0423207b62 73 w1 = start_Hz * 3.14159 * 2;
ms523 1:0f0423207b62 74 w2 = stop_Hz * 3.14159 * 2;
ms523 1:0f0423207b62 75 a = (w2 - w1) / (2 * run_time * 1000000);
ms523 1:0f0423207b62 76 b = w1 / 1000;
ms523 0:3a132f85c1a8 77
ms523 1:0f0423207b62 78 // Set the test running
ms523 1:0f0423207b62 79 running = 1;
ms523 1:0f0423207b62 80
ms523 1:0f0423207b62 81 while(1) {
ms523 1:0f0423207b62 82 ;
ms523 0:3a132f85c1a8 83 }
ms523 0:3a132f85c1a8 84
ms523 0:3a132f85c1a8 85 }