Vitec Group / Mbed 2 deprecated 1_AH10_Motor_Scope_Tool

As used for motor testing May / June 2016

Dependencies:   iC_MU mbed-rtos mbed

Fork of SweptSine by Vitec Group

main.cpp@1:0f0423207b62, 2016-03-22 (annotated)

Committer:
ms523
Date:
Tue Mar 22 10:57:41 2016 +0000
Revision:
1:0f0423207b62
Parent:
0:3a132f85c1a8
Child:
2:be3b6a72f823
working in velocity mode

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