Carbon Fibre / Mbed 2 deprecated Motor_test_harness

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Classic_PID iC_MU mbed-rtos mbed

main.cpp@1:1ad84260ff59, 2015-05-18 (annotated)

Committer:
ms523
Date:
Mon May 18 09:06:07 2015 +0000
Revision:
1:1ad84260ff59
Parent:
0:7ce0bc67f60f
Child:
2:dc684c402296
current latest code - not revision controlled

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 _Kp 0.18 // Kp.Freq product
ms523 0:7ce0bc67f60f 7 #define _Ki 0.006 // Ki.Freq product
ms523 0:7ce0bc67f60f 8
ms523 0:7ce0bc67f60f 9 Serial pc(USBTX,USBRX);
ms523 0:7ce0bc67f60f 10
ms523 0:7ce0bc67f60f 11 // iC-MU Encoder Objects
ms523 0:7ce0bc67f60f 12 iC_MU tilt_ic_mu(p5,p6,p7,p8);
ms523 0:7ce0bc67f60f 13 iC_MU TiltPos(p5,p6,p7,p11);
ms523 0:7ce0bc67f60f 14 iC_MU pan_ic_mu(p5,p6,p7,p12);
ms523 0:7ce0bc67f60f 15 iC_MU PanPos(p5,p6,p7,p13);
ms523 1:1ad84260ff59 16
ms523 1:1ad84260ff59 17 #ifdef TARGET_LPC4088
ms523 0:7ce0bc67f60f 18 // Tilt Motor
ms523 0:7ce0bc67f60f 19 PwmOut Tilt_Motor_PWM(p27); // Purple wire
ms523 0:7ce0bc67f60f 20 DigitalOut Tilt_Motor_Direction(p28); // Yellow wire
ms523 0:7ce0bc67f60f 21 // Pan Motor
ms523 0:7ce0bc67f60f 22 PwmOut Pan_Motor_PWM(p25); // Purple wire
ms523 0:7ce0bc67f60f 23 DigitalOut Pan_Motor_Direction(p26); // Yellow wire
ms523 1:1ad84260ff59 24 #endif
ms523 1:1ad84260ff59 25
ms523 1:1ad84260ff59 26 #ifdef TARGET_LPC1768
ms523 0:7ce0bc67f60f 27 // Tilt Motor
ms523 0:7ce0bc67f60f 28 PwmOut Tilt_Motor_PWM(p23); // Purple wire
ms523 0:7ce0bc67f60f 29 DigitalOut Tilt_Motor_Direction(p24); // Yellow wire
ms523 0:7ce0bc67f60f 30 // Pan Motor
ms523 0:7ce0bc67f60f 31 PwmOut Pan_Motor_PWM(p21); // Purple wire
ms523 0:7ce0bc67f60f 32 DigitalOut Pan_Motor_Direction(p22); // Yellow wire
ms523 1:1ad84260ff59 33 #endif
ms523 0:7ce0bc67f60f 34
ms523 0:7ce0bc67f60f 35 // Joystick stuff
ms523 0:7ce0bc67f60f 36 AnalogIn Pan_Joystick(p16);
ms523 0:7ce0bc67f60f 37 AnalogIn Tilt_Joystick(p17);
ms523 0:7ce0bc67f60f 38 AnalogIn Zoom_Joystick(p18);
ms523 0:7ce0bc67f60f 39 AnalogIn Focus_Pot(p19); // The top Pot (Pot 1)
ms523 0:7ce0bc67f60f 40
ms523 0:7ce0bc67f60f 41 // Camera Stuff
ms523 0:7ce0bc67f60f 42 Serial Camera(p9,p10);
ms523 0:7ce0bc67f60f 43 InterruptIn Switch(p14);
ms523 1:1ad84260ff59 44 #ifdef TARGET_LPC4088
ms523 1:1ad84260ff59 45 DigitalOut led4(LED1),led2(LED2), led3(LED3), led1(LED4);
ms523 1:1ad84260ff59 46 #endif
ms523 1:1ad84260ff59 47 #ifdef TARGET_LPC1768
ms523 0:7ce0bc67f60f 48 DigitalOut led1(LED1),led2(LED2), led3(LED3), led4(LED4);
ms523 1:1ad84260ff59 49 #endif
ms523 1:1ad84260ff59 50
ms523 1:1ad84260ff59 51
ms523 0:7ce0bc67f60f 52 #define AUTO 1
ms523 0:7ce0bc67f60f 53 #define MANUAL 0
ms523 0:7ce0bc67f60f 54
ms523 0:7ce0bc67f60f 55 /* Kp = 0.00018, Ki = 0.000006, Kd = 0.0, 0.0001 */
ms523 0:7ce0bc67f60f 56 Classic_PID PanVelocityPID(0.00018, 0.000006, 0.0, 0.0001); //Kp, ki, kd, kvelff
ms523 0:7ce0bc67f60f 57 Classic_PID TiltVelocityPID(0.00018, 0.000006, 0.0, 0.0001); //Kp, ki, kd, kvelff
ms523 0:7ce0bc67f60f 58
ms523 0:7ce0bc67f60f 59 // Globals
ms523 1:1ad84260ff59 60 int ZoomPos = 10248; // Strat off at max Zoom position to avoid jerks on startup
ms523 0:7ce0bc67f60f 61 bool AutofocusFlag = true;
ms523 0:7ce0bc67f60f 62 bool ManualfocusFlag = false;
ms523 0:7ce0bc67f60f 63 bool Mode = AUTO;
ms523 0:7ce0bc67f60f 64 bool joystick = true;
ms523 0:7ce0bc67f60f 65
ms523 0:7ce0bc67f60f 66 extern int LastPanPosition;
ms523 0:7ce0bc67f60f 67 extern int LastTiltPosition;
ms523 1:1ad84260ff59 68 float Pan_JoyStickDem = 0.5;
ms523 1:1ad84260ff59 69 float Tilt_JoyStickDem = 0.5;
ms523 0:7ce0bc67f60f 70
ms523 0:7ce0bc67f60f 71 void PanVelocityLoop(void const *args);
ms523 0:7ce0bc67f60f 72 void TiltVelocityLoop(void const *args);
ms523 1:1ad84260ff59 73 int ServiceKeyboard();
ms523 0:7ce0bc67f60f 74 void UpdateCamera(float, float);
ms523 0:7ce0bc67f60f 75
ms523 0:7ce0bc67f60f 76 void isr_Switch(void)
ms523 0:7ce0bc67f60f 77 {
ms523 0:7ce0bc67f60f 78 wait_ms(1); // Wait 1ms and check if the switch is still pressed
ms523 0:7ce0bc67f60f 79 if (Switch) {
ms523 0:7ce0bc67f60f 80 wait_ms(1); // Wait another 1ms and double check the switch
ms523 0:7ce0bc67f60f 81 if (Switch) {
ms523 0:7ce0bc67f60f 82 Mode = !Mode;
ms523 0:7ce0bc67f60f 83 if(Mode == AUTO) {
ms523 0:7ce0bc67f60f 84 AutofocusFlag = true;
ms523 0:7ce0bc67f60f 85 } else {
ms523 0:7ce0bc67f60f 86 ManualfocusFlag = true;
ms523 0:7ce0bc67f60f 87 }
ms523 0:7ce0bc67f60f 88 }
ms523 0:7ce0bc67f60f 89 }
ms523 1:1ad84260ff59 90 }
ms523 0:7ce0bc67f60f 91
ms523 0:7ce0bc67f60f 92 void Anti_Lock(void const *args)
ms523 0:7ce0bc67f60f 93 {
ms523 0:7ce0bc67f60f 94 Pan_Motor_Direction = !Pan_Motor_Direction.read(); // Toggle motor direction to overcome lock protection
ms523 0:7ce0bc67f60f 95 Tilt_Motor_Direction = !Tilt_Motor_Direction.read();
ms523 0:7ce0bc67f60f 96 wait_us(10);
ms523 0:7ce0bc67f60f 97 Pan_Motor_Direction = !Pan_Motor_Direction.read();
ms523 0:7ce0bc67f60f 98 Tilt_Motor_Direction = !Tilt_Motor_Direction.read();
ms523 0:7ce0bc67f60f 99 }
ms523 0:7ce0bc67f60f 100
ms523 1:1ad84260ff59 101
ms523 1:1ad84260ff59 102 // IRQ for Rx camera interrupt
ms523 1:1ad84260ff59 103 void UART3_rxInterrupt(void)
ms523 1:1ad84260ff59 104 {
ms523 1:1ad84260ff59 105 // Create an array to read the whole message
ms523 1:1ad84260ff59 106 static int index = 0;
ms523 1:1ad84260ff59 107 static char msg[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
ms523 1:1ad84260ff59 108
ms523 1:1ad84260ff59 109 NVIC_DisableIRQ(UART3_IRQn); // Disable the interrupt
ms523 1:1ad84260ff59 110 uint32_t IRR3 = LPC_UART3->IIR;
ms523 1:1ad84260ff59 111 msg[index] = LPC_UART3->RBR; // Read the byte and put it into the next available element of the array
ms523 1:1ad84260ff59 112
ms523 1:1ad84260ff59 113 // Check to see if that is the end of the message
ms523 1:1ad84260ff59 114 if(msg[index] == 0xFF) {
ms523 1:1ad84260ff59 115 // Check to see if the response is zoom data i.e. 7 bytes long and starts with 0x90, 0x50...
ms523 1:1ad84260ff59 116 if(index == 6 && msg[0] == 0x90 && msg[1] == 0x50) {
ms523 1:1ad84260ff59 117 ZoomPos = (msg[2] & 0x0F) << 12;
ms523 1:1ad84260ff59 118 ZoomPos += (msg[3] & 0x0F) << 8;
ms523 1:1ad84260ff59 119 ZoomPos += (msg[4] & 0x0F) << 4;
ms523 1:1ad84260ff59 120 ZoomPos += (msg[5] & 0x0F);
ms523 1:1ad84260ff59 121 }
ms523 1:1ad84260ff59 122 index = 0;
ms523 1:1ad84260ff59 123 } else {
ms523 1:1ad84260ff59 124 index++;
ms523 1:1ad84260ff59 125 }
ms523 1:1ad84260ff59 126
ms523 1:1ad84260ff59 127 led3 = !led3; // Flash the LED
ms523 1:1ad84260ff59 128 NVIC_EnableIRQ(UART3_IRQn); // Enable the interrupt again
ms523 1:1ad84260ff59 129 }
ms523 1:1ad84260ff59 130
ms523 0:7ce0bc67f60f 131 int main()
ms523 0:7ce0bc67f60f 132 {
ms523 0:7ce0bc67f60f 133 // Increase the Camera baud rate
ms523 0:7ce0bc67f60f 134 Camera.baud(38400);
ms523 1:1ad84260ff59 135
ms523 1:1ad84260ff59 136 // Set up the Camera Rx Interrupt
ms523 1:1ad84260ff59 137 Camera.attach(&UART3_rxInterrupt,Serial::RxIrq);
ms523 0:7ce0bc67f60f 138
ms523 0:7ce0bc67f60f 139 // Set up the switch to toggle autofocus
ms523 0:7ce0bc67f60f 140 Switch.mode(PullDown); //Set the internal pull down resistor
ms523 0:7ce0bc67f60f 141 Switch.rise(&isr_Switch); //ISR for the switch
ms523 0:7ce0bc67f60f 142 led4 = 1; // We start in autofocus mode
ms523 0:7ce0bc67f60f 143 led1 = 1; // Turn all other LEDs off
ms523 0:7ce0bc67f60f 144 led2 = 1;
ms523 0:7ce0bc67f60f 145 led3 = 0;
ms523 1:1ad84260ff59 146
ms523 0:7ce0bc67f60f 147 // Set up the Pan motor
ms523 0:7ce0bc67f60f 148 Pan_Motor_PWM.period_us(50); // Set PWM to 20 kHz
ms523 0:7ce0bc67f60f 149 Pan_Motor_PWM = 1; // Start with motor static
ms523 0:7ce0bc67f60f 150
ms523 0:7ce0bc67f60f 151 // Set up the Tilt motor
ms523 0:7ce0bc67f60f 152 Tilt_Motor_PWM.period_us(50); // Set PWM to 20 kHz
ms523 0:7ce0bc67f60f 153 Tilt_Motor_PWM = 1; // Start with motor static
ms523 0:7ce0bc67f60f 154
ms523 0:7ce0bc67f60f 155 // Initalise Pan Velocity loop RtosTimer thread
ms523 0:7ce0bc67f60f 156 RtosTimer PanVelocityLoop_timer(PanVelocityLoop, osTimerPeriodic);
ms523 0:7ce0bc67f60f 157 PanVelocityLoop_timer.start(1); // Run at 1kHz
ms523 0:7ce0bc67f60f 158
ms523 0:7ce0bc67f60f 159 // Initalise Tilt Velocity loop RtosTimer thread
ms523 0:7ce0bc67f60f 160 RtosTimer TiltVelocityLoop_timer(TiltVelocityLoop, osTimerPeriodic);
ms523 0:7ce0bc67f60f 161 TiltVelocityLoop_timer.start(1); // Run at 1kHz
ms523 0:7ce0bc67f60f 162
ms523 0:7ce0bc67f60f 163 // Initalise Pan PID Loop
ms523 0:7ce0bc67f60f 164 PanVelocityPID.setProcessLimits(1.0, -1.0);
ms523 0:7ce0bc67f60f 165 PanVelocityPID.setSetPoint(0);
ms523 0:7ce0bc67f60f 166 LastPanPosition = pan_ic_mu.ReadPOSITION() >> 1;
ms523 0:7ce0bc67f60f 167
ms523 0:7ce0bc67f60f 168 // Initalise Tilt PID Loop
ms523 0:7ce0bc67f60f 169 TiltVelocityPID.setProcessLimits(1.0, -1.0);
ms523 0:7ce0bc67f60f 170 TiltVelocityPID.setSetPoint(0);
ms523 0:7ce0bc67f60f 171 LastTiltPosition = tilt_ic_mu.ReadPOSITION() >> 1;
ms523 0:7ce0bc67f60f 172
ms523 0:7ce0bc67f60f 173 // Initalise Anti-Lock RtosTimer thread
ms523 0:7ce0bc67f60f 174 RtosTimer Anti_Lock_timer(Anti_Lock, osTimerPeriodic);
ms523 0:7ce0bc67f60f 175 Anti_Lock_timer.start(1000); // Run at 1Hz
ms523 1:1ad84260ff59 176
ms523 1:1ad84260ff59 177 int tiltPosition;
ms523 1:1ad84260ff59 178
ms523 0:7ce0bc67f60f 179 while(1) {
ms523 1:1ad84260ff59 180 // Check to see if a key has been pressed
ms523 1:1ad84260ff59 181 if(pc.readable()) {
ms523 1:1ad84260ff59 182 ServiceKeyboard();
ms523 1:1ad84260ff59 183 }
ms523 1:1ad84260ff59 184
ms523 1:1ad84260ff59 185 Thread::wait(50);
ms523 0:7ce0bc67f60f 186 // Update the Zoom and Focus Demands
ms523 0:7ce0bc67f60f 187 UpdateCamera(Zoom_Joystick.read(),Focus_Pot.read());
ms523 0:7ce0bc67f60f 188
ms523 1:1ad84260ff59 189 //pc.printf("\n\r %d ",tiltPosition);
ms523 1:1ad84260ff59 190 Thread::wait(50);
ms523 1:1ad84260ff59 191
ms523 0:7ce0bc67f60f 192 if(joystick) {
ms523 1:1ad84260ff59 193 // Apply Offset
ms523 1:1ad84260ff59 194 Pan_JoyStickDem = Pan_Joystick.read() - 0.5;
ms523 1:1ad84260ff59 195 Tilt_JoyStickDem = Tilt_Joystick.read() - 0.5;
ms523 1:1ad84260ff59 196 // Square the demand for pan joystick profile & check if neg
ms523 1:1ad84260ff59 197 if(Pan_JoyStickDem > 0) {
ms523 1:1ad84260ff59 198 Pan_JoyStickDem = Pan_JoyStickDem * Pan_JoyStickDem;
ms523 1:1ad84260ff59 199 } else {
ms523 1:1ad84260ff59 200 Pan_JoyStickDem = Pan_JoyStickDem * Pan_JoyStickDem * -1;
ms523 1:1ad84260ff59 201 }
ms523 1:1ad84260ff59 202 // Square the demand for tilt joystick profile & check if neg
ms523 1:1ad84260ff59 203 tiltPosition = TiltPos.ReadPOSITION();
ms523 1:1ad84260ff59 204 if(Tilt_JoyStickDem < 0) {
ms523 1:1ad84260ff59 205 // Check the tilt angle to see if it is within softlimits
ms523 1:1ad84260ff59 206 if(tiltPosition < 170000) {
ms523 1:1ad84260ff59 207 Tilt_JoyStickDem = Tilt_JoyStickDem * Tilt_JoyStickDem;
ms523 1:1ad84260ff59 208 } else {
ms523 1:1ad84260ff59 209 Tilt_JoyStickDem = 0.0;
ms523 1:1ad84260ff59 210 }
ms523 1:1ad84260ff59 211 } else {
ms523 1:1ad84260ff59 212 // Check the tilt angle to see if it is within softlimits
ms523 1:1ad84260ff59 213 if(tiltPosition > 25000) {
ms523 1:1ad84260ff59 214 Tilt_JoyStickDem = Tilt_JoyStickDem * Tilt_JoyStickDem * -1;
ms523 1:1ad84260ff59 215 } else {
ms523 1:1ad84260ff59 216 Tilt_JoyStickDem = 0.0;
ms523 1:1ad84260ff59 217 }
ms523 1:1ad84260ff59 218 }
ms523 1:1ad84260ff59 219 // Apply scalefactor
ms523 1:1ad84260ff59 220 Pan_JoyStickDem *= 20000; // Apply scalefactor
ms523 1:1ad84260ff59 221 Tilt_JoyStickDem *= 20000; // Apply scalefactor
ms523 1:1ad84260ff59 222 // Apply Zoom %
ms523 1:1ad84260ff59 223 Pan_JoyStickDem /= (ZoomPos >> 9) + 1; // Catch divide by zeros
ms523 1:1ad84260ff59 224 Tilt_JoyStickDem /= (ZoomPos >> 9) + 1; // Catch divide by zeros
ms523 1:1ad84260ff59 225
ms523 1:1ad84260ff59 226 PanVelocityPID.setSetPoint((int)Pan_JoyStickDem); // Read the joystick and apply the gain
ms523 1:1ad84260ff59 227 TiltVelocityPID.setSetPoint((int)Tilt_JoyStickDem); // Read the joystick and apply the gain
ms523 1:1ad84260ff59 228 }
ms523 1:1ad84260ff59 229
ms523 0:7ce0bc67f60f 230 if(AutofocusFlag) {
ms523 0:7ce0bc67f60f 231 // Turn on the autofocus
ms523 1:1ad84260ff59 232 led1 = 1; // Turn the LED on to show we are in auto-focus mode
ms523 0:7ce0bc67f60f 233 Camera.putc(0x81); // Camera
ms523 0:7ce0bc67f60f 234 Camera.putc(0x01); // Turn on auto-focus cmd
ms523 0:7ce0bc67f60f 235 Camera.putc(0x04);
ms523 0:7ce0bc67f60f 236 Camera.putc(0x38);
ms523 0:7ce0bc67f60f 237 Camera.putc(0x02);
ms523 0:7ce0bc67f60f 238 Camera.putc(0xFF); // Terminator
ms523 0:7ce0bc67f60f 239 AutofocusFlag = false;
ms523 0:7ce0bc67f60f 240 Mode = AUTO;
ms523 0:7ce0bc67f60f 241 }
ms523 0:7ce0bc67f60f 242 if(ManualfocusFlag) {
ms523 0:7ce0bc67f60f 243 // Turn on Manual focus
ms523 1:1ad84260ff59 244 led1 = 0; // Turn the LED off to show we are in manual-focus mode
ms523 0:7ce0bc67f60f 245 Camera.putc(0x81); // Camera
ms523 0:7ce0bc67f60f 246 Camera.putc(0x01); // Turn on manual-focus cmd
ms523 0:7ce0bc67f60f 247 Camera.putc(0x04);
ms523 0:7ce0bc67f60f 248 Camera.putc(0x38);
ms523 0:7ce0bc67f60f 249 Camera.putc(0x03);
ms523 0:7ce0bc67f60f 250 Camera.putc(0xFF); // Terminator
ms523 0:7ce0bc67f60f 251 ManualfocusFlag = false;
ms523 0:7ce0bc67f60f 252 Mode = MANUAL;
ms523 1:1ad84260ff59 253 }
ms523 0:7ce0bc67f60f 254 }
ms523 0:7ce0bc67f60f 255 }
ms523 0:7ce0bc67f60f 256
ms523 1:1ad84260ff59 257 /*
ms523 1:1ad84260ff59 258 // Increase the serial baud rate
ms523 1:1ad84260ff59 259 // pc.baud(921600);
ms523 0:7ce0bc67f60f 260
ms523 1:1ad84260ff59 261 while(1) {
ms523 1:1ad84260ff59 262 Thread::wait(100);
ms523 0:7ce0bc67f60f 263
ms523 1:1ad84260ff59 264 if(pc.readable()) {
ms523 1:1ad84260ff59 265 ServiceKeyboard();
ms523 1:1ad84260ff59 266 }
ms523 1:1ad84260ff59 267
ms523 1:1ad84260ff59 268 if(Demand) {
ms523 1:1ad84260ff59 269 pc.printf("\n\r %0.3f ",PanVelocityPID.getSetPoint());
ms523 1:1ad84260ff59 270 }
ms523 1:1ad84260ff59 271 }
ms523 0:7ce0bc67f60f 272 }*/