Carbon Fibre / Mbed 2 deprecated Motor_test_harness

Test Program

Dependencies:   Classic_PID iC_MU mbed-rtos mbed

PanVelocityLoop.cpp@4:4dafa4113982, 2015-06-25 (annotated)

Committer:
acodd
Date:
Thu Jun 25 09:41:26 2015 +0000
Revision:
4:4dafa4113982
Parent:
0:7ce0bc67f60f 
Test Harness for a single axis

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ms523 0:7ce0bc67f60f 1 #include "mbed.h"
ms523 0:7ce0bc67f60f 2 #include "iC_MU.h"
ms523 0:7ce0bc67f60f 3 #include "rtos.h"
ms523 0:7ce0bc67f60f 4 #include "Classic_PID.h"
ms523 0:7ce0bc67f60f 5
ms523 0:7ce0bc67f60f 6 // Define limits for zero crossing
ms523 0:7ce0bc67f60f 7 // These values should allow operation upto 3750 RPM
ms523 0:7ce0bc67f60f 8 #define bits 18 // The number of bits we want to use
ms523 0:7ce0bc67f60f 9 #define OneTurn (1<<bits) // 262144 counts per rev
ms523 0:7ce0bc67f60f 10 #define Lower (1<<(bits-5)) // 8192 counts = 11.25 degrees
ms523 0:7ce0bc67f60f 11 #define Upper OneTurn - Lower // 262144 - 8192 = 253952
ms523 0:7ce0bc67f60f 12
ms523 0:7ce0bc67f60f 13 extern iC_MU pan_ic_mu;
ms523 0:7ce0bc67f60f 14 extern PwmOut Pan_Motor_PWM;
ms523 0:7ce0bc67f60f 15 extern DigitalOut Pan_Motor_Direction;
ms523 0:7ce0bc67f60f 16 extern Classic_PID PanVelocityPID;
ms523 0:7ce0bc67f60f 17
ms523 0:7ce0bc67f60f 18 int LastPanPosition = 0;
ms523 0:7ce0bc67f60f 19
ms523 0:7ce0bc67f60f 20 void PanVelocityLoop(void const *args)
ms523 0:7ce0bc67f60f 21 {
ms523 0:7ce0bc67f60f 22 int Position = pan_ic_mu.ReadPOSITION() >> (19 - bits); // Read the current position from the iC-MU and bitshift to reduce noise
ms523 0:7ce0bc67f60f 23 int Velocity = Position - LastPanPosition; // Calculate change in position (i.e. Velocity)
ms523 0:7ce0bc67f60f 24 float Duty_Cycle = 0.0;
ms523 0:7ce0bc67f60f 25
ms523 0:7ce0bc67f60f 26 // Check to see if we have gone past the index point
ms523 0:7ce0bc67f60f 27 if(Position < Lower & LastPanPosition > Upper) { // We have gone over the index point in 1 direction
ms523 0:7ce0bc67f60f 28 Velocity += OneTurn;
ms523 0:7ce0bc67f60f 29 } else if(Position > Upper & LastPanPosition < Lower) { // We have gone over the index point in the other direction
ms523 0:7ce0bc67f60f 30 Velocity -= OneTurn;
ms523 0:7ce0bc67f60f 31 }
ms523 0:7ce0bc67f60f 32 LastPanPosition = Position; // Update new position from next time
ms523 0:7ce0bc67f60f 33
ms523 0:7ce0bc67f60f 34 PanVelocityPID.setProcessValue(Velocity); // Pass the Velocity onto the PID loop
ms523 0:7ce0bc67f60f 35 //Duty_Cycle = VelocityPID.compute_ff(); // Adjust the PWM output with no feed forward
ms523 0:7ce0bc67f60f 36 Duty_Cycle = PanVelocityPID.compute();
ms523 0:7ce0bc67f60f 37
ms523 0:7ce0bc67f60f 38 if(Duty_Cycle < 0) {
ms523 0:7ce0bc67f60f 39 Pan_Motor_Direction = 0;
ms523 0:7ce0bc67f60f 40 Pan_Motor_PWM = 1 - (Duty_Cycle * -1.0);
ms523 0:7ce0bc67f60f 41 } else {
ms523 0:7ce0bc67f60f 42 Pan_Motor_Direction = 1;
ms523 0:7ce0bc67f60f 43 Pan_Motor_PWM = 1 - Duty_Cycle;
ms523 0:7ce0bc67f60f 44 }
ms523 0:7ce0bc67f60f 45 }