Vitec Group / Mbed 2 deprecated SweptSine

running on mbed now

Dependencies:   Classic_PID iC_MU mbed-rtos mbed

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

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

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 1:0f0423207b62 13 extern iC_MU icMu;
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 1:0f0423207b62 29 float Duty_Cycle = 0.0;
ms523 1:0f0423207b62 30
ms523 1:0f0423207b62 31 // Find the icMu data
ms523 1:0f0423207b62 32 int icMuPosition = icMu.ReadPOSITION() >> (19 - bits); // Read the current position from the iC-MU and bitshift to reduce noise
ms523 1:0f0423207b62 33 int icMuVelocity = icMuPosition - LasticMuPosition; // Calculate change in position (i.e. Velocity)
ms523 1:0f0423207b62 34
ms523 1:0f0423207b62 35 if(running){
ms523 1:0f0423207b62 36 pc.printf("\n\r%d %d",run_count,icMuPosition);
ms523 1:0f0423207b62 37
ms523 1:0f0423207b62 38 if(run_count < 5000){
ms523 1:0f0423207b62 39 PanVelocityPID.setSetPoint(amplitude * sin((a*run_count*run_count)+(b*run_count)));
ms523 1:0f0423207b62 40 run_count++; // Inc run_count
ms523 1:0f0423207b62 41 } else {
ms523 1:0f0423207b62 42 running = 0; // Stop the test
ms523 1:0f0423207b62 43 PanVelocityPID.setSetPoint(0);
ms523 1:0f0423207b62 44 }
ms523 1:0f0423207b62 45 }
ms523 1:0f0423207b62 46
ms523 1:0f0423207b62 47 // Check to see if the icMu has gone past the index point
ms523 1:0f0423207b62 48 if(icMuPosition < Lower & LasticMuPosition > Upper) { // The icMu has gone over the index point in 1 direction
ms523 1:0f0423207b62 49 icMuVelocity += OneTurn;
ms523 1:0f0423207b62 50 } else if(icMuPosition > Upper & LasticMuPosition < Lower) { // The icMu has gone over the index point in the other direction
ms523 1:0f0423207b62 51 icMuVelocity -= OneTurn;
ms523 1:0f0423207b62 52 }
ms523 1:0f0423207b62 53
ms523 1:0f0423207b62 54 LasticMuPosition = icMuPosition; // Update new position from next time
ms523 1:0f0423207b62 55
ms523 1:0f0423207b62 56 // Set the process value
ms523 1:0f0423207b62 57 PanVelocityPID.setProcessValue(icMuVelocity); // Pass the Velocity onto the PID loop
ms523 1:0f0423207b62 58
ms523 1:0f0423207b62 59
ms523 1:0f0423207b62 60 Duty_Cycle = PanVelocityPID.compute();
ms523 1:0f0423207b62 61
ms523 1:0f0423207b62 62 if(Duty_Cycle < 0) {
ms523 1:0f0423207b62 63 Pan_Motor_Direction = 1;
ms523 1:0f0423207b62 64 Pan_Motor_PWM = 1 - (Duty_Cycle * -1.0);
ms523 1:0f0423207b62 65 } else {
ms523 1:0f0423207b62 66 Pan_Motor_Direction = 0;
ms523 1:0f0423207b62 67 Pan_Motor_PWM = 1 - Duty_Cycle;
ms523 1:0f0423207b62 68 }
ms523 1:0f0423207b62 69 }