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@0:7ce0bc67f60f, 2015-04-02 (annotated)

Committer:
ms523
Date:
Thu Apr 02 07:35:39 2015 +0000
Revision:
0:7ce0bc67f60f
Child:
1:1ad84260ff59
Working as demo'd in the end of March sprint review.

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 0:7ce0bc67f60f 16 /*
ms523 0:7ce0bc67f60f 17 // Tilt Motor
ms523 0:7ce0bc67f60f 18 PwmOut Tilt_Motor_PWM(p27); // Purple wire
ms523 0:7ce0bc67f60f 19 DigitalOut Tilt_Motor_Direction(p28); // Yellow wire
ms523 0:7ce0bc67f60f 20
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 0:7ce0bc67f60f 24 */
ms523 0:7ce0bc67f60f 25 // Tilt Motor
ms523 0:7ce0bc67f60f 26 PwmOut Tilt_Motor_PWM(p23); // Purple wire
ms523 0:7ce0bc67f60f 27 DigitalOut Tilt_Motor_Direction(p24); // Yellow wire
ms523 0:7ce0bc67f60f 28
ms523 0:7ce0bc67f60f 29 // Pan Motor
ms523 0:7ce0bc67f60f 30 PwmOut Pan_Motor_PWM(p21); // Purple wire
ms523 0:7ce0bc67f60f 31 DigitalOut Pan_Motor_Direction(p22); // Yellow wire
ms523 0:7ce0bc67f60f 32
ms523 0:7ce0bc67f60f 33 // Joystick stuff
ms523 0:7ce0bc67f60f 34 AnalogIn Pan_Joystick(p16);
ms523 0:7ce0bc67f60f 35 AnalogIn Tilt_Joystick(p17);
ms523 0:7ce0bc67f60f 36 AnalogIn Zoom_Joystick(p18);
ms523 0:7ce0bc67f60f 37 AnalogIn Focus_Pot(p19); // The top Pot (Pot 1)
ms523 0:7ce0bc67f60f 38
ms523 0:7ce0bc67f60f 39 // Camera Stuff
ms523 0:7ce0bc67f60f 40 Serial Camera(p9,p10);
ms523 0:7ce0bc67f60f 41 InterruptIn Switch(p14);
ms523 0:7ce0bc67f60f 42 DigitalOut led1(LED1),led2(LED2), led3(LED3), led4(LED4);
ms523 0:7ce0bc67f60f 43 #define AUTO 1
ms523 0:7ce0bc67f60f 44 #define MANUAL 0
ms523 0:7ce0bc67f60f 45
ms523 0:7ce0bc67f60f 46 /* Kp = 0.00018, Ki = 0.000006, Kd = 0.0, 0.0001 */
ms523 0:7ce0bc67f60f 47 Classic_PID PanVelocityPID(0.00018, 0.000006, 0.0, 0.0001); //Kp, ki, kd, kvelff
ms523 0:7ce0bc67f60f 48 Classic_PID TiltVelocityPID(0.00018, 0.000006, 0.0, 0.0001); //Kp, ki, kd, kvelff
ms523 0:7ce0bc67f60f 49
ms523 0:7ce0bc67f60f 50 // Globals
ms523 0:7ce0bc67f60f 51 int ZoomPos = 0;
ms523 0:7ce0bc67f60f 52 bool AutofocusFlag = true;
ms523 0:7ce0bc67f60f 53 bool ManualfocusFlag = false;
ms523 0:7ce0bc67f60f 54 bool Mode = AUTO;
ms523 0:7ce0bc67f60f 55 bool joystick = true;
ms523 0:7ce0bc67f60f 56
ms523 0:7ce0bc67f60f 57 extern int LastPanPosition;
ms523 0:7ce0bc67f60f 58 extern int LastTiltPosition;
ms523 0:7ce0bc67f60f 59 float Pan_JoyStickDem = 0.0;
ms523 0:7ce0bc67f60f 60 float Tilt_JoyStickDem = 0.0;
ms523 0:7ce0bc67f60f 61
ms523 0:7ce0bc67f60f 62 void PanVelocityLoop(void const *args);
ms523 0:7ce0bc67f60f 63 void TiltVelocityLoop(void const *args);
ms523 0:7ce0bc67f60f 64 //int ServiceKeyboard();
ms523 0:7ce0bc67f60f 65 void UpdateCamera(float, float);
ms523 0:7ce0bc67f60f 66 /*
ms523 0:7ce0bc67f60f 67 // Code for interrupt on Rx from Camera
ms523 0:7ce0bc67f60f 68 extern "C" void MyUart3Handler(void)
ms523 0:7ce0bc67f60f 69 {
ms523 0:7ce0bc67f60f 70 static int index = 0;
ms523 0:7ce0bc67f60f 71 static char msg[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
ms523 0:7ce0bc67f60f 72
ms523 0:7ce0bc67f60f 73 // Read the byte and put it into the next available element of the array
ms523 0:7ce0bc67f60f 74 msg[index] = Camera.getc();
ms523 0:7ce0bc67f60f 75
ms523 0:7ce0bc67f60f 76 // Check to see if that is the end of the message
ms523 0:7ce0bc67f60f 77 if(msg[index] == 0xFF) {
ms523 0:7ce0bc67f60f 78 // Check to see if the response is zoom data i.e. 7 bytes long and starts with 0x90, 0x50...
ms523 0:7ce0bc67f60f 79 if(index == 6 && msg[0] == 0x90 && msg[1] == 0x50) {
ms523 0:7ce0bc67f60f 80 ZoomPos = (msg[2] & 0x0F) << 12;
ms523 0:7ce0bc67f60f 81 ZoomPos += (msg[3] & 0x0F) << 8;
ms523 0:7ce0bc67f60f 82 ZoomPos += (msg[4] & 0x0F) << 4;
ms523 0:7ce0bc67f60f 83 ZoomPos += (msg[5] & 0x0F);
ms523 0:7ce0bc67f60f 84 }
ms523 0:7ce0bc67f60f 85 index = 0;
ms523 0:7ce0bc67f60f 86 } else {
ms523 0:7ce0bc67f60f 87 index++;
ms523 0:7ce0bc67f60f 88 }
ms523 0:7ce0bc67f60f 89 }
ms523 0:7ce0bc67f60f 90
ms523 0:7ce0bc67f60f 91 void isr_Switch(void)
ms523 0:7ce0bc67f60f 92 {
ms523 0:7ce0bc67f60f 93 wait_ms(1); // Wait 1ms and check if the switch is still pressed
ms523 0:7ce0bc67f60f 94 if (Switch) {
ms523 0:7ce0bc67f60f 95 wait_ms(1); // Wait another 1ms and double check the switch
ms523 0:7ce0bc67f60f 96 if (Switch) {
ms523 0:7ce0bc67f60f 97 Mode = !Mode;
ms523 0:7ce0bc67f60f 98 if(Mode == AUTO) {
ms523 0:7ce0bc67f60f 99 AutofocusFlag = true;
ms523 0:7ce0bc67f60f 100 } else {
ms523 0:7ce0bc67f60f 101 ManualfocusFlag = true;
ms523 0:7ce0bc67f60f 102 }
ms523 0:7ce0bc67f60f 103 }
ms523 0:7ce0bc67f60f 104 }
ms523 0:7ce0bc67f60f 105 }*/
ms523 0:7ce0bc67f60f 106
ms523 0:7ce0bc67f60f 107 void Anti_Lock(void const *args)
ms523 0:7ce0bc67f60f 108 {
ms523 0:7ce0bc67f60f 109 Pan_Motor_Direction = !Pan_Motor_Direction.read(); // Toggle motor direction to overcome lock protection
ms523 0:7ce0bc67f60f 110 Tilt_Motor_Direction = !Tilt_Motor_Direction.read();
ms523 0:7ce0bc67f60f 111 wait_us(10);
ms523 0:7ce0bc67f60f 112 Pan_Motor_Direction = !Pan_Motor_Direction.read();
ms523 0:7ce0bc67f60f 113 Tilt_Motor_Direction = !Tilt_Motor_Direction.read();
ms523 0:7ce0bc67f60f 114 }
ms523 0:7ce0bc67f60f 115
ms523 0:7ce0bc67f60f 116 int main()
ms523 0:7ce0bc67f60f 117 {
ms523 0:7ce0bc67f60f 118 // Increase the Camera baud rate
ms523 0:7ce0bc67f60f 119 Camera.baud(38400);
ms523 0:7ce0bc67f60f 120 /*
ms523 0:7ce0bc67f60f 121 // Set up the Camera Rx Interrupt & enable it
ms523 0:7ce0bc67f60f 122 NVIC_SetVector(UART3_IRQn, (uint32_t)MyUart3Handler);
ms523 0:7ce0bc67f60f 123 NVIC_EnableIRQ(UART3_IRQn);
ms523 0:7ce0bc67f60f 124 LPC_UART3->IER = 0x01;
ms523 0:7ce0bc67f60f 125
ms523 0:7ce0bc67f60f 126 // Set up the switch to toggle autofocus
ms523 0:7ce0bc67f60f 127 Switch.mode(PullDown); //Set the internal pull down resistor
ms523 0:7ce0bc67f60f 128 Switch.rise(&isr_Switch); //ISR for the switch
ms523 0:7ce0bc67f60f 129 led4 = 1; // We start in autofocus mode
ms523 0:7ce0bc67f60f 130 led1 = 1; // Turn all other LEDs off
ms523 0:7ce0bc67f60f 131 led2 = 1;
ms523 0:7ce0bc67f60f 132 led3 = 0;
ms523 0:7ce0bc67f60f 133 */
ms523 0:7ce0bc67f60f 134 // Set up the Pan motor
ms523 0:7ce0bc67f60f 135 Pan_Motor_PWM.period_us(50); // Set PWM to 20 kHz
ms523 0:7ce0bc67f60f 136 Pan_Motor_PWM = 1; // Start with motor static
ms523 0:7ce0bc67f60f 137
ms523 0:7ce0bc67f60f 138 // Set up the Tilt motor
ms523 0:7ce0bc67f60f 139 Tilt_Motor_PWM.period_us(50); // Set PWM to 20 kHz
ms523 0:7ce0bc67f60f 140 Tilt_Motor_PWM = 1; // Start with motor static
ms523 0:7ce0bc67f60f 141
ms523 0:7ce0bc67f60f 142 // Initalise Pan Velocity loop RtosTimer thread
ms523 0:7ce0bc67f60f 143 RtosTimer PanVelocityLoop_timer(PanVelocityLoop, osTimerPeriodic);
ms523 0:7ce0bc67f60f 144 PanVelocityLoop_timer.start(1); // Run at 1kHz
ms523 0:7ce0bc67f60f 145
ms523 0:7ce0bc67f60f 146 // Initalise Tilt Velocity loop RtosTimer thread
ms523 0:7ce0bc67f60f 147 RtosTimer TiltVelocityLoop_timer(TiltVelocityLoop, osTimerPeriodic);
ms523 0:7ce0bc67f60f 148 TiltVelocityLoop_timer.start(1); // Run at 1kHz
ms523 0:7ce0bc67f60f 149
ms523 0:7ce0bc67f60f 150 // Initalise Pan PID Loop
ms523 0:7ce0bc67f60f 151 PanVelocityPID.setProcessLimits(1.0, -1.0);
ms523 0:7ce0bc67f60f 152 PanVelocityPID.setSetPoint(0);
ms523 0:7ce0bc67f60f 153 LastPanPosition = pan_ic_mu.ReadPOSITION() >> 1;
ms523 0:7ce0bc67f60f 154
ms523 0:7ce0bc67f60f 155 // Initalise Tilt PID Loop
ms523 0:7ce0bc67f60f 156 TiltVelocityPID.setProcessLimits(1.0, -1.0);
ms523 0:7ce0bc67f60f 157 TiltVelocityPID.setSetPoint(0);
ms523 0:7ce0bc67f60f 158 LastTiltPosition = tilt_ic_mu.ReadPOSITION() >> 1;
ms523 0:7ce0bc67f60f 159
ms523 0:7ce0bc67f60f 160 // Initalise Anti-Lock RtosTimer thread
ms523 0:7ce0bc67f60f 161 RtosTimer Anti_Lock_timer(Anti_Lock, osTimerPeriodic);
ms523 0:7ce0bc67f60f 162 Anti_Lock_timer.start(1000); // Run at 1Hz
ms523 0:7ce0bc67f60f 163
ms523 0:7ce0bc67f60f 164 while(1) {
ms523 0:7ce0bc67f60f 165 Thread::wait(100);
ms523 0:7ce0bc67f60f 166 // Update the Zoom and Focus Demands
ms523 0:7ce0bc67f60f 167 UpdateCamera(Zoom_Joystick.read(),Focus_Pot.read());
ms523 0:7ce0bc67f60f 168 Thread::wait(100);
ms523 0:7ce0bc67f60f 169
ms523 0:7ce0bc67f60f 170 if(joystick) {
ms523 0:7ce0bc67f60f 171 Pan_JoyStickDem = Pan_Joystick.read() - 0.5;
ms523 0:7ce0bc67f60f 172 Tilt_JoyStickDem = Tilt_Joystick.read() - 0.5;
ms523 0:7ce0bc67f60f 173 Pan_JoyStickDem *= 80;
ms523 0:7ce0bc67f60f 174 Tilt_JoyStickDem *= -80;
ms523 0:7ce0bc67f60f 175 // Square the demand for pan joystick profile
ms523 0:7ce0bc67f60f 176 if(Pan_JoyStickDem > 0) {
ms523 0:7ce0bc67f60f 177 Pan_JoyStickDem = Pan_JoyStickDem * Pan_JoyStickDem;
ms523 0:7ce0bc67f60f 178 } else {
ms523 0:7ce0bc67f60f 179 Pan_JoyStickDem = Pan_JoyStickDem * Pan_JoyStickDem * -1;
ms523 0:7ce0bc67f60f 180 }
ms523 0:7ce0bc67f60f 181 // Square the demand for tilt joystick profile
ms523 0:7ce0bc67f60f 182 if(Tilt_JoyStickDem > 0) {
ms523 0:7ce0bc67f60f 183 Tilt_JoyStickDem = Tilt_JoyStickDem * Tilt_JoyStickDem;
ms523 0:7ce0bc67f60f 184 } else {
ms523 0:7ce0bc67f60f 185 Tilt_JoyStickDem = Tilt_JoyStickDem * Tilt_JoyStickDem * -1;
ms523 0:7ce0bc67f60f 186 }
ms523 0:7ce0bc67f60f 187 PanVelocityPID.setSetPoint((int)Pan_JoyStickDem); // Read the joystick and apply the gain
ms523 0:7ce0bc67f60f 188 TiltVelocityPID.setSetPoint((int)Tilt_JoyStickDem); // Read the joystick and apply the gain
ms523 0:7ce0bc67f60f 189 }
ms523 0:7ce0bc67f60f 190 /*
ms523 0:7ce0bc67f60f 191 if(AutofocusFlag) {
ms523 0:7ce0bc67f60f 192 // Turn on the autofocus
ms523 0:7ce0bc67f60f 193 led4 = 1; // Turn the LED on to show we are in auto-focus mode
ms523 0:7ce0bc67f60f 194 Camera.putc(0x81); // Camera
ms523 0:7ce0bc67f60f 195 Camera.putc(0x01); // Turn on auto-focus cmd
ms523 0:7ce0bc67f60f 196 Camera.putc(0x04);
ms523 0:7ce0bc67f60f 197 Camera.putc(0x38);
ms523 0:7ce0bc67f60f 198 Camera.putc(0x02);
ms523 0:7ce0bc67f60f 199 Camera.putc(0xFF); // Terminator
ms523 0:7ce0bc67f60f 200 AutofocusFlag = false;
ms523 0:7ce0bc67f60f 201 Mode = AUTO;
ms523 0:7ce0bc67f60f 202 }
ms523 0:7ce0bc67f60f 203 if(ManualfocusFlag) {
ms523 0:7ce0bc67f60f 204 // Turn on Manual focus
ms523 0:7ce0bc67f60f 205 led4 = 0; // Turn the LED off to show we are in manual-focus mode
ms523 0:7ce0bc67f60f 206 Camera.putc(0x81); // Camera
ms523 0:7ce0bc67f60f 207 Camera.putc(0x01); // Turn on manual-focus cmd
ms523 0:7ce0bc67f60f 208 Camera.putc(0x04);
ms523 0:7ce0bc67f60f 209 Camera.putc(0x38);
ms523 0:7ce0bc67f60f 210 Camera.putc(0x03);
ms523 0:7ce0bc67f60f 211 Camera.putc(0xFF); // Terminator
ms523 0:7ce0bc67f60f 212 ManualfocusFlag = false;
ms523 0:7ce0bc67f60f 213 Mode = MANUAL;
ms523 0:7ce0bc67f60f 214 }*/
ms523 0:7ce0bc67f60f 215 }
ms523 0:7ce0bc67f60f 216 }
ms523 0:7ce0bc67f60f 217
ms523 0:7ce0bc67f60f 218 /*
ms523 0:7ce0bc67f60f 219 // Increase the serial baud rate
ms523 0:7ce0bc67f60f 220 // pc.baud(921600);
ms523 0:7ce0bc67f60f 221
ms523 0:7ce0bc67f60f 222 while(1) {
ms523 0:7ce0bc67f60f 223 Thread::wait(100);
ms523 0:7ce0bc67f60f 224
ms523 0:7ce0bc67f60f 225 if(pc.readable()) {
ms523 0:7ce0bc67f60f 226 ServiceKeyboard();
ms523 0:7ce0bc67f60f 227 }
ms523 0:7ce0bc67f60f 228 if(Capture) {
ms523 0:7ce0bc67f60f 229 // Print to terminal the current motor encoder positions
ms523 0:7ce0bc67f60f 230 pc.printf("\n\r %d %d ",pan_ic_mu.ReadPOSITION(),tilt_ic_mu.ReadPOSITION());
ms523 0:7ce0bc67f60f 231 }
ms523 0:7ce0bc67f60f 232
ms523 0:7ce0bc67f60f 233 if(Demand) {
ms523 0:7ce0bc67f60f 234 pc.printf("\n\r %0.3f ",PanVelocityPID.getSetPoint());
ms523 0:7ce0bc67f60f 235 }
ms523 0:7ce0bc67f60f 236 }
ms523 0:7ce0bc67f60f 237 }*/