Carbon Fibre
Test Program
Dependencies: Classic_PID iC_MU mbed-rtos mbed
Revision 0:7ce0bc67f60f, committed 2015-04-02
- Comitter:
- ms523
- Date:
- Thu Apr 02 07:35:39 2015 +0000
- Child:
- 1:1ad84260ff59
- Commit message:
- Working as demo'd in the end of March sprint review.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Classic_PID.lib Thu Apr 02 07:35:39 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/Vitec-Group/code/Classic_PID/#7b42de70b65f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PanVelocityLoop.cpp Thu Apr 02 07:35:39 2015 +0000
@@ -0,0 +1,45 @@
+#include "mbed.h"
+#include "iC_MU.h"
+#include "rtos.h"
+#include "Classic_PID.h"
+
+// Define limits for zero crossing
+// These values should allow operation upto 3750 RPM
+#define bits 18 // The number of bits we want to use
+#define OneTurn (1<<bits) // 262144 counts per rev
+#define Lower (1<<(bits-5)) // 8192 counts = 11.25 degrees
+#define Upper OneTurn - Lower // 262144 - 8192 = 253952
+
+extern iC_MU pan_ic_mu;
+extern PwmOut Pan_Motor_PWM;
+extern DigitalOut Pan_Motor_Direction;
+extern Classic_PID PanVelocityPID;
+
+int LastPanPosition = 0;
+
+void PanVelocityLoop(void const *args)
+{
+ int Position = pan_ic_mu.ReadPOSITION() >> (19 - bits); // Read the current position from the iC-MU and bitshift to reduce noise
+ int Velocity = Position - LastPanPosition; // Calculate change in position (i.e. Velocity)
+ float Duty_Cycle = 0.0;
+
+ // Check to see if we have gone past the index point
+ if(Position < Lower & LastPanPosition > Upper) { // We have gone over the index point in 1 direction
+ Velocity += OneTurn;
+ } else if(Position > Upper & LastPanPosition < Lower) { // We have gone over the index point in the other direction
+ Velocity -= OneTurn;
+ }
+ LastPanPosition = Position; // Update new position from next time
+
+ PanVelocityPID.setProcessValue(Velocity); // Pass the Velocity onto the PID loop
+ //Duty_Cycle = VelocityPID.compute_ff(); // Adjust the PWM output with no feed forward
+ Duty_Cycle = PanVelocityPID.compute();
+
+ if(Duty_Cycle < 0) {
+ Pan_Motor_Direction = 0;
+ Pan_Motor_PWM = 1 - (Duty_Cycle * -1.0);
+ } else {
+ Pan_Motor_Direction = 1;
+ Pan_Motor_PWM = 1 - Duty_Cycle;
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ServiceKeyboard.cpp Thu Apr 02 07:35:39 2015 +0000
@@ -0,0 +1,98 @@
+#include "mbed.h"
+#include "iC_MU.h"
+#include "rtos.h"
+#include "Classic_PID.h"
+
+extern Serial pc;
+extern Classic_PID PanVelocityPID;
+extern Classic_PID TiltVelocityPID;
+
+extern bool joystick;
+
+// Returns the new set point entered via the keyboard
+int ServiceKeyboard ()
+{
+ return(1);
+}
+ /*
+ int key; // The keypress by the user
+ int value = 0; // The variable to return
+
+ key = pc.getc(); // Read the intial keypress
+
+ if (key == 'j') {
+ joystick = !joystick;
+ if(joystick) {
+ pc.printf("\n\rJoystick");
+ } else {
+ pc.printf("\n\rPC");
+ }
+ }
+ // Set Velocity for Pan and Tilt here...
+ else if(key == 'p') {
+ // We are going to set the speed of pan...
+ pc.printf("\n\r New Pan Vel: ");
+ key = pc.getc();
+ if (key == 0x1B) {
+ if(pc.getc() == 0x4F) {
+ if(pc.getc() == 0x53) {
+ pc.printf("-");
+ do {
+ key = pc.getc(); // Wait for a keypress
+ if(key >= '0' && key <= '9') { // Check it is a number
+ value *= 10; // Index the old number
+ value += (key - '0'); // Add on the new number
+ pc.printf("%c",key); // Display the new number
+ }
+ } while(key != 0x0D);
+ PanVelocityPID.setSetPoint(value * -1);
+ }
+ }
+ } else if(key >= '0' && key <= '9') { // Check it is a number
+ pc.printf("%c",key);
+ value = (key - '0');
+ do {
+ key = pc.getc(); // Wait for a keypress
+ if(key >= '0' && key <= '9') { // Check it is a number
+ value *= 10; // Index the old number
+ value += (key - '0'); // Add on the new number
+ pc.printf("%c",key); // Display the new number
+ }
+ } while(key != 0x0D);
+ PanVelocityPID.setSetPoint(value);
+ }
+ } else if(key == 't') {
+ // We are going to set the speed of tilt...
+ pc.printf("\n\r New Tilt Vel: ");
+ key = pc.getc();
+ if (key == 0x1B) {
+ if(pc.getc() == 0x4F) {
+ if(pc.getc() == 0x53) {
+ pc.printf("-");
+ do {
+ key = pc.getc(); // Wait for a keypress
+ if(key >= '0' && key <= '9') { // Check it is a number
+ value *= 10; // Index the old number
+ value += (key - '0'); // Add on the new number
+ pc.printf("%c",key); // Display the new number
+ }
+ } while(key != 0x0D);
+ TiltVelocityPID.setSetPoint(value * -1);
+ }
+ }
+ } else if(key >= '0' && key <= '9') { // Check it is a number
+ pc.printf("%c",key);
+ value = (key - '0');
+ do {
+ key = pc.getc(); // Wait for a keypress
+ if(key >= '0' && key <= '9') { // Check it is a number
+ value *= 10; // Index the old number
+ value += (key - '0'); // Add on the new number
+ pc.printf("%c",key); // Display the new number
+ }
+ } while(key != 0x0D);
+ TiltVelocityPID.setSetPoint(value);
+ }
+ }
+ return(1);
+}*/
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TiltVelocityLoop.cpp Thu Apr 02 07:35:39 2015 +0000
@@ -0,0 +1,44 @@
+#include "mbed.h"
+#include "iC_MU.h"
+#include "rtos.h"
+#include "Classic_PID.h"
+
+// Define limits for zero crossing
+// These values should allow operation upto 3750 RPM
+#define bits 18 // The number of bits we want to use
+#define OneTurn (1<<bits) // 262144 counts per rev
+#define Lower (1<<(bits-5)) // 8192 counts = 11.25 degrees
+#define Upper OneTurn - Lower // 262144 - 8192 = 253952
+
+extern iC_MU tilt_ic_mu;
+extern PwmOut Tilt_Motor_PWM;
+extern DigitalOut Tilt_Motor_Direction;
+extern Classic_PID TiltVelocityPID;
+
+int LastTiltPosition = 0;
+
+void TiltVelocityLoop(void const *args)
+{
+ int Position = tilt_ic_mu.ReadPOSITION() >> (19 - bits);// Read the current position from the iC-MU and bitshift to reduce noise
+ int Velocity = Position - LastTiltPosition; // Calculate change in position (i.e. Velocity)
+ float Duty_Cycle = 0.0;
+
+ // Check to see if we have gone past the index point
+ if(Position < Lower & LastTiltPosition > Upper) { // We have gone over the index point in 1 direction
+ Velocity += OneTurn;
+ } else if(Position > Upper & LastTiltPosition < Lower) {// We have gone over the index point in the other direction
+ Velocity -= OneTurn;
+ }
+ LastTiltPosition = Position; // Update new position from next time
+
+ TiltVelocityPID.setProcessValue(Velocity); // Pass the Velocity onto the PID loop
+ Duty_Cycle = TiltVelocityPID.compute();
+
+ if(Duty_Cycle < 0) {
+ Tilt_Motor_Direction = 0;
+ Tilt_Motor_PWM = 1 - (Duty_Cycle * -1.0);
+ } else {
+ Tilt_Motor_Direction = 1;
+ Tilt_Motor_PWM = 1 - Duty_Cycle;
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/UpdateCamera.cpp Thu Apr 02 07:35:39 2015 +0000
@@ -0,0 +1,70 @@
+#include "mbed.h"
+#include "rtos.h"
+
+#define AUTO 1
+#define MANUAL 0
+
+extern Serial Camera;
+extern bool Mode;
+
+void UpdateCamera (float ZoomDemand, float FocusDemand)
+{
+ static signed int LastZoom = 0;
+ static signed int LastFocus = 0;
+
+ // The ZoomDemand value passed to this function is a raw read of the AnalogIn object
+ ZoomDemand -= 0.5; // Shift the raw value to the centre-tap
+ ZoomDemand *= -14; // Scale to get in the -7 to +7 range
+ int Zoom = (int)ZoomDemand; // Cast to an int
+
+ // Do the same thing for Focus
+ FocusDemand *= 0x7000; // Scale to the range of 0 to 28672
+ FocusDemand += 0x0800; // Add a shift of 2048 to get to 2048 to 30720
+ int Focus = (int)FocusDemand & 0xFFC0; // Knock off the last 6 bits of noise!
+
+ // See if we need to move the Camera Zoom Position
+ if(Zoom != LastZoom) {
+
+ // First calculate the new Zoom Setting
+ if (Zoom > 0) {
+ // Send the tele command
+ Zoom += 0x20;
+ } else if(Zoom < 0) {
+ // Send the wide command
+ Zoom *= -1;
+ Zoom += 0x30;
+ }
+ LastZoom = Zoom; // Update the last know Focus Demand for next time
+
+ // Send first 4 characters (always the same)
+ Camera.putc(0x81);
+ Camera.putc(0x01);
+ Camera.putc(0x04);
+ Camera.putc(0x07);
+ Camera.putc(Zoom);
+ Camera.putc(0xFF);
+ }
+
+ // Now see if we need to move the Camera Focus Position
+ if(Mode == MANUAL && Focus != LastFocus) {
+ Camera.putc(0x81); // Camera
+ Camera.putc(0x01);
+ Camera.putc(0x04);
+ Camera.putc(0x48); // Direct focus cmd
+ Camera.putc((Focus & 0xF000)>>12); // p variable
+ Camera.putc((Focus & 0x0F00)>>8); // q variable
+ Camera.putc((Focus & 0x00F0)>>4); // r variable
+ Camera.putc(Focus & 0x000F); // s variable
+ Camera.putc(0xFF);
+
+ LastFocus = Focus; // Update the last know Focus Demand for next time
+ }
+
+ // Return the current Zoom position
+ Camera.putc(0x81); // Address byte from host to camera - 0x81
+ Camera.putc(0x09); // Zoom position inquiry - 8x 09 04 47 ff
+ Camera.putc(0x04);
+ Camera.putc(0x47);
+ Camera.putc(0xFF); // Terminator byte - all 1s
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/iC_MU.lib Thu Apr 02 07:35:39 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/Vitec-Group/code/iC_MU/#ae562ccce5bd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Apr 02 07:35:39 2015 +0000
@@ -0,0 +1,237 @@
+#include "mbed.h"
+#include "iC_MU.h"
+#include "rtos.h"
+#include "Classic_PID.h"
+
+#define _Kp 0.18 // Kp.Freq product
+#define _Ki 0.006 // Ki.Freq product
+
+Serial pc(USBTX,USBRX);
+
+// iC-MU Encoder Objects
+iC_MU tilt_ic_mu(p5,p6,p7,p8);
+iC_MU TiltPos(p5,p6,p7,p11);
+iC_MU pan_ic_mu(p5,p6,p7,p12);
+iC_MU PanPos(p5,p6,p7,p13);
+/*
+// Tilt Motor
+PwmOut Tilt_Motor_PWM(p27); // Purple wire
+DigitalOut Tilt_Motor_Direction(p28); // Yellow wire
+
+// Pan Motor
+PwmOut Pan_Motor_PWM(p25); // Purple wire
+DigitalOut Pan_Motor_Direction(p26); // Yellow wire
+*/
+// Tilt Motor
+PwmOut Tilt_Motor_PWM(p23); // Purple wire
+DigitalOut Tilt_Motor_Direction(p24); // Yellow wire
+
+// Pan Motor
+PwmOut Pan_Motor_PWM(p21); // Purple wire
+DigitalOut Pan_Motor_Direction(p22); // Yellow wire
+
+// Joystick stuff
+AnalogIn Pan_Joystick(p16);
+AnalogIn Tilt_Joystick(p17);
+AnalogIn Zoom_Joystick(p18);
+AnalogIn Focus_Pot(p19); // The top Pot (Pot 1)
+
+// Camera Stuff
+Serial Camera(p9,p10);
+InterruptIn Switch(p14);
+DigitalOut led1(LED1),led2(LED2), led3(LED3), led4(LED4);
+#define AUTO 1
+#define MANUAL 0
+
+/* Kp = 0.00018, Ki = 0.000006, Kd = 0.0, 0.0001 */
+Classic_PID PanVelocityPID(0.00018, 0.000006, 0.0, 0.0001); //Kp, ki, kd, kvelff
+Classic_PID TiltVelocityPID(0.00018, 0.000006, 0.0, 0.0001); //Kp, ki, kd, kvelff
+
+// Globals
+int ZoomPos = 0;
+bool AutofocusFlag = true;
+bool ManualfocusFlag = false;
+bool Mode = AUTO;
+bool joystick = true;
+
+extern int LastPanPosition;
+extern int LastTiltPosition;
+float Pan_JoyStickDem = 0.0;
+float Tilt_JoyStickDem = 0.0;
+
+void PanVelocityLoop(void const *args);
+void TiltVelocityLoop(void const *args);
+//int ServiceKeyboard();
+void UpdateCamera(float, float);
+/*
+// Code for interrupt on Rx from Camera
+extern "C" void MyUart3Handler(void)
+{
+ static int index = 0;
+ static char msg[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+
+ // Read the byte and put it into the next available element of the array
+ msg[index] = Camera.getc();
+
+ // Check to see if that is the end of the message
+ if(msg[index] == 0xFF) {
+ // Check to see if the response is zoom data i.e. 7 bytes long and starts with 0x90, 0x50...
+ if(index == 6 && msg[0] == 0x90 && msg[1] == 0x50) {
+ ZoomPos = (msg[2] & 0x0F) << 12;
+ ZoomPos += (msg[3] & 0x0F) << 8;
+ ZoomPos += (msg[4] & 0x0F) << 4;
+ ZoomPos += (msg[5] & 0x0F);
+ }
+ index = 0;
+ } else {
+ index++;
+ }
+}
+
+void isr_Switch(void)
+{
+ wait_ms(1); // Wait 1ms and check if the switch is still pressed
+ if (Switch) {
+ wait_ms(1); // Wait another 1ms and double check the switch
+ if (Switch) {
+ Mode = !Mode;
+ if(Mode == AUTO) {
+ AutofocusFlag = true;
+ } else {
+ ManualfocusFlag = true;
+ }
+ }
+ }
+}*/
+
+void Anti_Lock(void const *args)
+{
+ Pan_Motor_Direction = !Pan_Motor_Direction.read(); // Toggle motor direction to overcome lock protection
+ Tilt_Motor_Direction = !Tilt_Motor_Direction.read();
+ wait_us(10);
+ Pan_Motor_Direction = !Pan_Motor_Direction.read();
+ Tilt_Motor_Direction = !Tilt_Motor_Direction.read();
+}
+
+int main()
+{
+ // Increase the Camera baud rate
+ Camera.baud(38400);
+/*
+ // Set up the Camera Rx Interrupt & enable it
+ NVIC_SetVector(UART3_IRQn, (uint32_t)MyUart3Handler);
+ NVIC_EnableIRQ(UART3_IRQn);
+ LPC_UART3->IER = 0x01;
+
+ // Set up the switch to toggle autofocus
+ Switch.mode(PullDown); //Set the internal pull down resistor
+ Switch.rise(&isr_Switch); //ISR for the switch
+ led4 = 1; // We start in autofocus mode
+ led1 = 1; // Turn all other LEDs off
+ led2 = 1;
+ led3 = 0;
+*/
+ // Set up the Pan motor
+ Pan_Motor_PWM.period_us(50); // Set PWM to 20 kHz
+ Pan_Motor_PWM = 1; // Start with motor static
+
+ // Set up the Tilt motor
+ Tilt_Motor_PWM.period_us(50); // Set PWM to 20 kHz
+ Tilt_Motor_PWM = 1; // Start with motor static
+
+ // Initalise Pan Velocity loop RtosTimer thread
+ RtosTimer PanVelocityLoop_timer(PanVelocityLoop, osTimerPeriodic);
+ PanVelocityLoop_timer.start(1); // Run at 1kHz
+
+ // Initalise Tilt Velocity loop RtosTimer thread
+ RtosTimer TiltVelocityLoop_timer(TiltVelocityLoop, osTimerPeriodic);
+ TiltVelocityLoop_timer.start(1); // Run at 1kHz
+
+ // Initalise Pan PID Loop
+ PanVelocityPID.setProcessLimits(1.0, -1.0);
+ PanVelocityPID.setSetPoint(0);
+ LastPanPosition = pan_ic_mu.ReadPOSITION() >> 1;
+
+ // Initalise Tilt PID Loop
+ TiltVelocityPID.setProcessLimits(1.0, -1.0);
+ TiltVelocityPID.setSetPoint(0);
+ LastTiltPosition = tilt_ic_mu.ReadPOSITION() >> 1;
+
+ // Initalise Anti-Lock RtosTimer thread
+ RtosTimer Anti_Lock_timer(Anti_Lock, osTimerPeriodic);
+ Anti_Lock_timer.start(1000); // Run at 1Hz
+
+ while(1) {
+ Thread::wait(100);
+ // Update the Zoom and Focus Demands
+ UpdateCamera(Zoom_Joystick.read(),Focus_Pot.read());
+ Thread::wait(100);
+
+ if(joystick) {
+ Pan_JoyStickDem = Pan_Joystick.read() - 0.5;
+ Tilt_JoyStickDem = Tilt_Joystick.read() - 0.5;
+ Pan_JoyStickDem *= 80;
+ Tilt_JoyStickDem *= -80;
+ // Square the demand for pan joystick profile
+ if(Pan_JoyStickDem > 0) {
+ Pan_JoyStickDem = Pan_JoyStickDem * Pan_JoyStickDem;
+ } else {
+ Pan_JoyStickDem = Pan_JoyStickDem * Pan_JoyStickDem * -1;
+ }
+ // Square the demand for tilt joystick profile
+ if(Tilt_JoyStickDem > 0) {
+ Tilt_JoyStickDem = Tilt_JoyStickDem * Tilt_JoyStickDem;
+ } else {
+ Tilt_JoyStickDem = Tilt_JoyStickDem * Tilt_JoyStickDem * -1;
+ }
+ PanVelocityPID.setSetPoint((int)Pan_JoyStickDem); // Read the joystick and apply the gain
+ TiltVelocityPID.setSetPoint((int)Tilt_JoyStickDem); // Read the joystick and apply the gain
+ }
+/*
+ if(AutofocusFlag) {
+ // Turn on the autofocus
+ led4 = 1; // Turn the LED on to show we are in auto-focus mode
+ Camera.putc(0x81); // Camera
+ Camera.putc(0x01); // Turn on auto-focus cmd
+ Camera.putc(0x04);
+ Camera.putc(0x38);
+ Camera.putc(0x02);
+ Camera.putc(0xFF); // Terminator
+ AutofocusFlag = false;
+ Mode = AUTO;
+ }
+ if(ManualfocusFlag) {
+ // Turn on Manual focus
+ led4 = 0; // Turn the LED off to show we are in manual-focus mode
+ Camera.putc(0x81); // Camera
+ Camera.putc(0x01); // Turn on manual-focus cmd
+ Camera.putc(0x04);
+ Camera.putc(0x38);
+ Camera.putc(0x03);
+ Camera.putc(0xFF); // Terminator
+ ManualfocusFlag = false;
+ Mode = MANUAL;
+ }*/
+ }
+}
+
+ /*
+ // Increase the serial baud rate
+ // pc.baud(921600);
+
+ while(1) {
+ Thread::wait(100);
+
+ if(pc.readable()) {
+ ServiceKeyboard();
+ }
+ if(Capture) {
+ // Print to terminal the current motor encoder positions
+ pc.printf("\n\r %d %d ",pan_ic_mu.ReadPOSITION(),tilt_ic_mu.ReadPOSITION());
+ }
+
+ if(Demand) {
+ pc.printf("\n\r %0.3f ",PanVelocityPID.getSetPoint());
+ }
+ }
+}*/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Thu Apr 02 07:35:39 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#d3d0e710b443
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 02 07:35:39 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e188a91d3eaa \ No newline at end of file