Vitec Group / Mbed 2 deprecated SweptSine

running on mbed now

Dependencies:   Classic_PID iC_MU mbed-rtos mbed

PanVelocityLoop.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 1:0f0423207b62 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 // Define limits for zero crossing
ms523 1:0f0423207b62 7 // These values should allow operation upto 3750 RPM
ms523 1:0f0423207b62 8 #define bits 18 // The number of bits we want to use
ms523 1:0f0423207b62 9 #define OneTurn (1<<bits) // 262144 counts per rev
ms523 1:0f0423207b62 10 #define Lower (1<<(bits-5)) // 8192 counts = 11.25 degrees
ms523 1:0f0423207b62 11 #define Upper OneTurn - Lower // 262144 - 8192 = 253952
ms523 1:0f0423207b62 12
ms523 2:be3b6a72f823 13 extern iC_MU icMu, icMu_Output;
ms523 1:0f0423207b62 14 extern PwmOut Pan_Motor_PWM;
ms523 1:0f0423207b62 15 extern DigitalOut Pan_Motor_Direction;
ms523 1:0f0423207b62 16 extern Classic_PID PanVelocityPID;
ms523 1:0f0423207b62 17 extern Serial pc;
ms523 1:0f0423207b62 18
ms523 1:0f0423207b62 19 extern bool running;
ms523 1:0f0423207b62 20 extern float amplitude;
ms523 1:0f0423207b62 21 extern float a;
ms523 1:0f0423207b62 22 extern float b;
ms523 1:0f0423207b62 23
ms523 1:0f0423207b62 24 int LasticMuPosition = 0;
ms523 1:0f0423207b62 25 int run_count = 0;
ms523 1:0f0423207b62 26
ms523 1:0f0423207b62 27 void PanVelocityLoop(void const *args)
ms523 1:0f0423207b62 28 {
ms523 2:be3b6a72f823 29 float Duty_Cycle = 0.0;
ms523 2:be3b6a72f823 30
ms523 2:be3b6a72f823 31 // Find the icMu data
ms523 2:be3b6a72f823 32 int icMuPosition = icMu.ReadPOSITION(); // Read the current position from the iC-MU
ms523 2:be3b6a72f823 33 PanVelocityPID.setProcessValue(icMuPosition); // Pass the Position onto the PID loop
ms523 2:be3b6a72f823 34
ms523 2:be3b6a72f823 35 //int icMuPosition = icMu.ReadPOSITION() >> (19 - bits); // Read the current position from the iC-MU and bitshift to reduce noise
ms523 2:be3b6a72f823 36 //int icMuVelocity = icMuPosition - LasticMuPosition; // Calculate change in position (i.e. Velocity)
ms523 2:be3b6a72f823 37
ms523 2:be3b6a72f823 38 if(running) {
ms523 2:be3b6a72f823 39 pc.printf("\n\r%d %d %d",run_count,icMuPosition,icMu_Output.ReadPOSITION());
ms523 2:be3b6a72f823 40
ms523 2:be3b6a72f823 41 if(run_count < 5000) {
ms523 2:be3b6a72f823 42 PanVelocityPID.setSetPoint(262144 + (amplitude * sin((a*run_count*run_count)+(b*run_count))));
ms523 2:be3b6a72f823 43 run_count++; // Inc run_count
ms523 1:0f0423207b62 44 } else {
ms523 1:0f0423207b62 45 running = 0; // Stop the test
ms523 2:be3b6a72f823 46 //PanVelocityPID.setSetPoint(0);
ms523 1:0f0423207b62 47 }
ms523 1:0f0423207b62 48 }
ms523 2:be3b6a72f823 49 /*
ms523 2:be3b6a72f823 50 // Check to see if the icMu has gone past the index point
ms523 2:be3b6a72f823 51 if(icMuPosition < Lower & LasticMuPosition > Upper) { // The icMu has gone over the index point in 1 direction
ms523 2:be3b6a72f823 52 icMuVelocity += OneTurn;
ms523 2:be3b6a72f823 53 } else if(icMuPosition > Upper & LasticMuPosition < Lower) { // The icMu has gone over the index point in the other direction
ms523 2:be3b6a72f823 54 icMuVelocity -= OneTurn;
ms523 2:be3b6a72f823 55 }
ms523 1:0f0423207b62 56
ms523 2:be3b6a72f823 57 LasticMuPosition = icMuPosition; // Update new position from next time
ms523 2:be3b6a72f823 58
ms523 2:be3b6a72f823 59 // Set the process value
ms523 2:be3b6a72f823 60 PanVelocityPID.setProcessValue(icMuVelocity); // Pass the Velocity onto the PID loop
ms523 2:be3b6a72f823 61 */
ms523 2:be3b6a72f823 62
ms523 2:be3b6a72f823 63 Duty_Cycle = PanVelocityPID.compute_ff();
ms523 1:0f0423207b62 64
ms523 1:0f0423207b62 65 if(Duty_Cycle < 0) {
ms523 1:0f0423207b62 66 Pan_Motor_Direction = 1;
ms523 1:0f0423207b62 67 Pan_Motor_PWM = 1 - (Duty_Cycle * -1.0);
ms523 1:0f0423207b62 68 } else {
ms523 1:0f0423207b62 69 Pan_Motor_Direction = 0;
ms523 1:0f0423207b62 70 Pan_Motor_PWM = 1 - Duty_Cycle;
ms523 1:0f0423207b62 71 }
ms523 1:0f0423207b62 72 }