First issue to MK
Dependencies: mbed QEI LM75B C12832_lcd2 FatFileSystem MSCFileSystem
main.cpp@3:b4435a45fdc7, 2019-01-23 (annotated)
- Committer:
- timdennis
- Date:
- Wed Jan 23 07:23:10 2019 +0000
- Revision:
- 3:b4435a45fdc7
- Parent:
- 2:c887cf1c3ed9
First issue to MK
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
timdennis | 3:b4435a45fdc7 | 1 | /* |
timdennis | 3:b4435a45fdc7 | 2 | -------------------------------------------------------------------------------- |
timdennis | 3:b4435a45fdc7 | 3 | CONFIDENTIAL |
timdennis | 3:b4435a45fdc7 | 4 | Project no. 1368 |
timdennis | 3:b4435a45fdc7 | 5 | Author: Tim Dennis |
timdennis | 3:b4435a45fdc7 | 6 | Copyright (c) 2016 Renishaw plc |
timdennis | 3:b4435a45fdc7 | 7 | -------------------------------------------------------------------------------- |
timdennis | 3:b4435a45fdc7 | 8 | PH10 Lock Motor test rig (Nodding Donkey) |
timdennis | 3:b4435a45fdc7 | 9 | A test rig to test the life of PH10 lock motor assemblies |
timdennis | 3:b4435a45fdc7 | 10 | Initially, this rig will evaluate updated Faulhaber motors (9/9/2016) |
timdennis | 3:b4435a45fdc7 | 11 | Info from Paul Boughey [pboughey@ems-ltd.com]… |
timdennis | 3:b4435a45fdc7 | 12 | The new 1516 SR design is more powerful than the current 1616E and also has a |
timdennis | 3:b4435a45fdc7 | 13 | number of modifications to its commutation system that should give a marked |
timdennis | 3:b4435a45fdc7 | 14 | improvement in the lifetime compared to the current motor/gearbox assembly. |
timdennis | 3:b4435a45fdc7 | 15 | -------------------------------------------------------------------------------- |
timdennis | 3:b4435a45fdc7 | 16 | Function... |
timdennis | 3:b4435a45fdc7 | 17 | Upon Power up, the firmware will automatically commence cycling. This is to cater |
timdennis | 3:b4435a45fdc7 | 18 | for power cuts, which could otherwise terminate a test, losing valuable test time. |
timdennis | 3:b4435a45fdc7 | 19 | The MBED will log data to USB stick. Should a power cycle occur, the USB stick |
timdennis | 3:b4435a45fdc7 | 20 | will enable the test to continue where it left off. |
timdennis | 3:b4435a45fdc7 | 21 | -------------------------------------------------------------------------------- |
timdennis | 3:b4435a45fdc7 | 22 | Displayed Data on LCD... |
timdennis | 3:b4435a45fdc7 | 23 | Firmware Version |
timdennis | 3:b4435a45fdc7 | 24 | Firmware Status |
timdennis | 3:b4435a45fdc7 | 25 | Lock/Unlock Cycles completed |
timdennis | 3:b4435a45fdc7 | 26 | Time to transit Opto slot [ms] |
timdennis | 3:b4435a45fdc7 | 27 | Average current whilst transitting Opto Slot [ms] |
timdennis | 3:b4435a45fdc7 | 28 | -------------------------------------------------------------------------------- |
timdennis | 3:b4435a45fdc7 | 29 | Data logged to USB Stick... |
timdennis | 3:b4435a45fdc7 | 30 | Firmware Version |
timdennis | 3:b4435a45fdc7 | 31 | Power Cycles (A marked is placed in file to indicate power cycle occured) |
timdennis | 3:b4435a45fdc7 | 32 | Lock/Unlock cycles completed (Target is 1 million) |
timdennis | 3:b4435a45fdc7 | 33 | Time to transit Opto slot [ms] |
timdennis | 3:b4435a45fdc7 | 34 | Average current whilst transitting Opto Slot [ms] |
timdennis | 3:b4435a45fdc7 | 35 | -------------------------------------------------------------------------------- |
timdennis | 3:b4435a45fdc7 | 36 | Version history: |
timdennis | 3:b4435a45fdc7 | 37 | 1.0 Dev version used on Nodding Donkey A-1368-0700-01 |
timdennis | 3:b4435a45fdc7 | 38 | 1.1 First used on Nodding Donkey A-1368-0700-02 |
timdennis | 3:b4435a45fdc7 | 39 | 1.2 Added QEI code which 'hijacks' LED pins 2 and 4 on MBED |
timdennis | 3:b4435a45fdc7 | 40 | 1.3 Added support for analogue POT for transducing CAM Wheel position |
timdennis | 3:b4435a45fdc7 | 41 | 1.4 Amended Unlock state machine to 'BRAKE' after 531 ms (rather than run-on) |
timdennis | 3:b4435a45fdc7 | 42 | 1.5 Added power-up diaplay of pot voltage (BIST) |
timdennis | 3:b4435a45fdc7 | 43 | 1.6 Fixed POT angle measurment bug, removed hall sensor code |
timdennis | 3:b4435a45fdc7 | 44 | 1.7 Reduced time spent between cycles |
timdennis | 3:b4435a45fdc7 | 45 | 1.8 Changed cycle repeat time to 4 seconds |
timdennis | 3:b4435a45fdc7 | 46 | 1.9 Changed elapsed time recording to use MBED RTC (although without battery, it's volatile) |
timdennis | 3:b4435a45fdc7 | 47 | 1.10 Tidied up 'header rows' |
timdennis | 3:b4435a45fdc7 | 48 | 1.11 JM wants cycle time of 10s reducing/improving. |
timdennis | 3:b4435a45fdc7 | 49 | Also added FW_Ver to logfile (FW changes mid life test are therefore recorded) |
timdennis | 3:b4435a45fdc7 | 50 | 1.12 Changed 'Joystick Off' action so that motion ceases at Locked position only |
timdennis | 3:b4435a45fdc7 | 51 | Also changed PCB_Temp_C to 'integer' as float precision wasn't required. |
timdennis | 3:b4435a45fdc7 | 52 | Also moved left column of FW_Ver in log file |
timdennis | 3:b4435a45fdc7 | 53 | 1.13 Now store Cycle Count at top of Nodder.csv, rather than counting the rows |
timdennis | 3:b4435a45fdc7 | 54 | in Nodder.csv as this had become very slow on power-up. |
timdennis | 3:b4435a45fdc7 | 55 | |
timdennis | 3:b4435a45fdc7 | 56 | */ |
timdennis | 3:b4435a45fdc7 | 57 | |
chris | 2:c887cf1c3ed9 | 58 | #include "mbed.h" |
timdennis | 3:b4435a45fdc7 | 59 | #include "C12832_lcd.h" |
timdennis | 3:b4435a45fdc7 | 60 | #include "Small_7.h" |
timdennis | 3:b4435a45fdc7 | 61 | #include "Arial_9.h" |
timdennis | 3:b4435a45fdc7 | 62 | #include "stdio.h" |
timdennis | 3:b4435a45fdc7 | 63 | #include <string> |
chris | 2:c887cf1c3ed9 | 64 | #include "MSCFileSystem.h" |
timdennis | 3:b4435a45fdc7 | 65 | #include "LM75B.h" |
timdennis | 3:b4435a45fdc7 | 66 | |
timdennis | 3:b4435a45fdc7 | 67 | #define FirmwareVersion "1.13" |
timdennis | 3:b4435a45fdc7 | 68 | #define HeaderRows 15 // number of header rows in output file (before data starts) |
timdennis | 3:b4435a45fdc7 | 69 | |
timdennis | 3:b4435a45fdc7 | 70 | // From C-1368-0700-02-A, the following conversions were calculated... |
timdennis | 3:b4435a45fdc7 | 71 | // Lockmotor: 4.864864 Volts per Amp |
timdennis | 3:b4435a45fdc7 | 72 | // ADC FSD: 65535 == 3.3 V |
timdennis | 3:b4435a45fdc7 | 73 | // 96612 ADC counts = 1000 mA |
timdennis | 3:b4435a45fdc7 | 74 | // 1 ADC count = 0.0103507 mA |
timdennis | 3:b4435a45fdc7 | 75 | |
timdennis | 3:b4435a45fdc7 | 76 | #define I_MOT_CONV 96.612 // Motor Current [mA] |
timdennis | 3:b4435a45fdc7 | 77 | #define V_REF_CONV 18.810 // V Ref [V] |
timdennis | 3:b4435a45fdc7 | 78 | #define DEFLECTION_CONV 1.000 // Hall sensor |
timdennis | 3:b4435a45fdc7 | 79 | |
timdennis | 3:b4435a45fdc7 | 80 | #define time_25ms 25 |
timdennis | 3:b4435a45fdc7 | 81 | #define time_50ms 50 |
timdennis | 3:b4435a45fdc7 | 82 | #define time_100ms 100 |
timdennis | 3:b4435a45fdc7 | 83 | #define time_500ms 500 |
timdennis | 3:b4435a45fdc7 | 84 | #define time_531ms 531 |
timdennis | 3:b4435a45fdc7 | 85 | |
timdennis | 3:b4435a45fdc7 | 86 | #define time_600ms 600 |
timdennis | 3:b4435a45fdc7 | 87 | #define time_1s 1000 |
timdennis | 3:b4435a45fdc7 | 88 | #define time_2s 3000 |
timdennis | 3:b4435a45fdc7 | 89 | #define time_4s 4000 |
timdennis | 3:b4435a45fdc7 | 90 | #define time_5s 5000 |
timdennis | 3:b4435a45fdc7 | 91 | #define time_6s 6000 |
timdennis | 3:b4435a45fdc7 | 92 | #define time_7s 7000 |
timdennis | 3:b4435a45fdc7 | 93 | #define time_8s 8000 |
timdennis | 3:b4435a45fdc7 | 94 | #define time_10s 10000 |
timdennis | 3:b4435a45fdc7 | 95 | |
timdennis | 3:b4435a45fdc7 | 96 | C12832_LCD lcd; // LCD object |
timdennis | 3:b4435a45fdc7 | 97 | |
timdennis | 3:b4435a45fdc7 | 98 | // MBED I/O... |
timdennis | 3:b4435a45fdc7 | 99 | DigitalIn OPTO_IN(p13); |
timdennis | 3:b4435a45fdc7 | 100 | DigitalOut RUN_OUT(p9); |
timdennis | 3:b4435a45fdc7 | 101 | DigitalOut BRK_OUT(p10); |
timdennis | 3:b4435a45fdc7 | 102 | DigitalOut HI_THRESHOLD_OUT(p30); |
timdennis | 3:b4435a45fdc7 | 103 | DigitalIn HI_LOCK_CUR_IN(p29); |
timdennis | 3:b4435a45fdc7 | 104 | AnalogIn I_MOTOR_ADC(p17); |
timdennis | 3:b4435a45fdc7 | 105 | AnalogIn V_REF_ADC(p16); |
timdennis | 3:b4435a45fdc7 | 106 | AnalogIn V_DEFLECTION_ADC(p18); // Hall sensor to transduce deflection |
timdennis | 3:b4435a45fdc7 | 107 | |
timdennis | 3:b4435a45fdc7 | 108 | // MBED Joystick... |
timdennis | 3:b4435a45fdc7 | 109 | DigitalIn SWITCH_UP(p15); // Stop Lock.Unlock operation |
timdennis | 3:b4435a45fdc7 | 110 | DigitalIn SWITCH_DN(p12); // Resume operation |
timdennis | 3:b4435a45fdc7 | 111 | |
timdennis | 3:b4435a45fdc7 | 112 | // MBED Pot useful during debugging... |
timdennis | 3:b4435a45fdc7 | 113 | AnalogIn Pot1(p19); |
chris | 2:c887cf1c3ed9 | 114 | |
timdennis | 3:b4435a45fdc7 | 115 | AnalogIn CAM_POT(p20); // Pot2 removed and Cam Angle Pot wired into here! |
timdennis | 3:b4435a45fdc7 | 116 | |
timdennis | 3:b4435a45fdc7 | 117 | // MBED LEDs... |
timdennis | 3:b4435a45fdc7 | 118 | BusOut LED_Pattern(LED4,LED3,LED2,LED1); |
timdennis | 3:b4435a45fdc7 | 119 | BusOut LED_TriColour(p23,p24,p25); |
timdennis | 3:b4435a45fdc7 | 120 | |
timdennis | 3:b4435a45fdc7 | 121 | LM75B tmp(p28,p27); |
timdennis | 3:b4435a45fdc7 | 122 | |
timdennis | 3:b4435a45fdc7 | 123 | int LED_TriColourCounter = 0; |
timdennis | 3:b4435a45fdc7 | 124 | |
timdennis | 3:b4435a45fdc7 | 125 | // Functions to control Motor to prevent MOSFET Shoot-through |
timdennis | 3:b4435a45fdc7 | 126 | void Brake_On(void); |
timdennis | 3:b4435a45fdc7 | 127 | void Brake_Off(void); |
timdennis | 3:b4435a45fdc7 | 128 | void Motor_On(void); |
timdennis | 3:b4435a45fdc7 | 129 | void Motor_Off(void); |
timdennis | 3:b4435a45fdc7 | 130 | |
timdennis | 3:b4435a45fdc7 | 131 | // Ticker ISR |
timdennis | 3:b4435a45fdc7 | 132 | void Lock_Motor_Ticker(void); |
timdennis | 3:b4435a45fdc7 | 133 | Ticker Lock_Ticker; |
timdennis | 3:b4435a45fdc7 | 134 | |
timdennis | 3:b4435a45fdc7 | 135 | void Init_LCD(void); |
timdennis | 3:b4435a45fdc7 | 136 | |
timdennis | 3:b4435a45fdc7 | 137 | int ticker_counter = 0; |
timdennis | 3:b4435a45fdc7 | 138 | int ticker_counter_p = 0; |
timdennis | 3:b4435a45fdc7 | 139 | int ticker_dwell = 0; |
timdennis | 3:b4435a45fdc7 | 140 | |
timdennis | 3:b4435a45fdc7 | 141 | int ticker_cycle_counter = 0; |
timdennis | 3:b4435a45fdc7 | 142 | |
timdennis | 3:b4435a45fdc7 | 143 | time_t tSeconds_elapsed; |
timdennis | 3:b4435a45fdc7 | 144 | int iSeconds_elapsed; |
timdennis | 3:b4435a45fdc7 | 145 | |
timdennis | 3:b4435a45fdc7 | 146 | time_t tSeconds_elapsed_p; |
timdennis | 3:b4435a45fdc7 | 147 | int iSeconds_elapsed_p; |
timdennis | 3:b4435a45fdc7 | 148 | |
timdennis | 3:b4435a45fdc7 | 149 | int iSeconds_per_cycle; |
timdennis | 3:b4435a45fdc7 | 150 | int iUSB_Write_ms; |
timdennis | 3:b4435a45fdc7 | 151 | int iUSB_Start_Time; |
timdennis | 3:b4435a45fdc7 | 152 | int iUSB_End_Time; |
timdennis | 3:b4435a45fdc7 | 153 | |
timdennis | 3:b4435a45fdc7 | 154 | |
timdennis | 3:b4435a45fdc7 | 155 | int cycle_counter = 0; |
timdennis | 3:b4435a45fdc7 | 156 | int cycle_counter_1s = 0; |
timdennis | 3:b4435a45fdc7 | 157 | int cycle_counter_k = 0; |
timdennis | 3:b4435a45fdc7 | 158 | |
timdennis | 3:b4435a45fdc7 | 159 | int LCD_update_time = 0; |
timdennis | 3:b4435a45fdc7 | 160 | int LCD_update_time_p = 0; |
timdennis | 3:b4435a45fdc7 | 161 | int LCD_update_counter = 0; |
timdennis | 3:b4435a45fdc7 | 162 | |
timdennis | 3:b4435a45fdc7 | 163 | int Current_Raw = 0; |
timdennis | 3:b4435a45fdc7 | 164 | int Current_mA = 0; |
timdennis | 3:b4435a45fdc7 | 165 | int Opto_Slot_ms = 0; |
timdennis | 3:b4435a45fdc7 | 166 | |
timdennis | 3:b4435a45fdc7 | 167 | int Current_At_Opto_Start = 0; |
timdennis | 3:b4435a45fdc7 | 168 | int Current_At_Cam_Start_Target = 0; |
timdennis | 3:b4435a45fdc7 | 169 | int Current_At_Cam_Start_Captured = 0; |
timdennis | 3:b4435a45fdc7 | 170 | int Current_Delta = 4000; // An offset to capture current when cam starts |
timdennis | 3:b4435a45fdc7 | 171 | |
timdennis | 3:b4435a45fdc7 | 172 | int Opto_To_Cam = 0; |
timdennis | 3:b4435a45fdc7 | 173 | |
timdennis | 3:b4435a45fdc7 | 174 | int iPCB_Temp_C = 0; |
timdennis | 3:b4435a45fdc7 | 175 | |
timdennis | 3:b4435a45fdc7 | 176 | int vref_counter = 0; |
timdennis | 3:b4435a45fdc7 | 177 | |
timdennis | 3:b4435a45fdc7 | 178 | int POT_Loop; |
timdennis | 3:b4435a45fdc7 | 179 | int POT_tmp; |
timdennis | 3:b4435a45fdc7 | 180 | |
timdennis | 3:b4435a45fdc7 | 181 | |
timdennis | 3:b4435a45fdc7 | 182 | bool Run_Status = 0; |
timdennis | 3:b4435a45fdc7 | 183 | bool New_Log = 0; |
timdennis | 3:b4435a45fdc7 | 184 | |
timdennis | 3:b4435a45fdc7 | 185 | int iCam_Angle_Degrees_Power_Up; |
timdennis | 3:b4435a45fdc7 | 186 | int iCam_Angle_Degrees_Opto_Start; |
timdennis | 3:b4435a45fdc7 | 187 | int iCam_Angle_Degrees_Opto_End; |
timdennis | 3:b4435a45fdc7 | 188 | int iCam_Angle_Degrees_Window; |
timdennis | 3:b4435a45fdc7 | 189 | int iCam_Angle_Degrees_Lock_Mot_Off; |
timdennis | 3:b4435a45fdc7 | 190 | int iCam_Angle_Degrees_Lock_Mot_Rest; |
timdennis | 3:b4435a45fdc7 | 191 | int iCam_Angle_Degrees_Lock_RunOn; |
timdennis | 3:b4435a45fdc7 | 192 | int iCam_Angle_Degrees_Unlocked_Mot_Off; |
timdennis | 3:b4435a45fdc7 | 193 | int iCam_Angle_Degrees_Unlocked_Rest; |
timdennis | 3:b4435a45fdc7 | 194 | int iCam_Angle_Degrees_Unlock_RunOn; |
timdennis | 3:b4435a45fdc7 | 195 | |
timdennis | 3:b4435a45fdc7 | 196 | std::string sLCD_Text_1("ROW 1"); |
timdennis | 3:b4435a45fdc7 | 197 | std::string sLCD_Text_2("ROW 2"); |
timdennis | 3:b4435a45fdc7 | 198 | |
timdennis | 3:b4435a45fdc7 | 199 | // State machine to control locking and unlocking... |
timdennis | 3:b4435a45fdc7 | 200 | enum Lock_State_Type |
timdennis | 3:b4435a45fdc7 | 201 | { |
timdennis | 3:b4435a45fdc7 | 202 | State_L_Halt, |
timdennis | 3:b4435a45fdc7 | 203 | State_L_Init, |
timdennis | 3:b4435a45fdc7 | 204 | State_L_Hi_Thresh_Low, |
timdennis | 3:b4435a45fdc7 | 205 | State_L_Release_Brake, |
timdennis | 3:b4435a45fdc7 | 206 | State_L_Motor_On, |
timdennis | 3:b4435a45fdc7 | 207 | State_L_Ignore_Curr_For_time_100ms, |
timdennis | 3:b4435a45fdc7 | 208 | State_L_Seek_Opto_Start, |
timdennis | 3:b4435a45fdc7 | 209 | State_L_Seek_Opto_End, |
timdennis | 3:b4435a45fdc7 | 210 | State_L_Dwell_After_Opto_End, |
timdennis | 3:b4435a45fdc7 | 211 | State_L_Motor_Off, |
timdennis | 3:b4435a45fdc7 | 212 | State_L_Brake_On, |
timdennis | 3:b4435a45fdc7 | 213 | State_L_Dwell_At_Locked, |
timdennis | 3:b4435a45fdc7 | 214 | State_U_Init, |
timdennis | 3:b4435a45fdc7 | 215 | State_U_Hi_Thresh_Low, |
timdennis | 3:b4435a45fdc7 | 216 | State_U_Release_Brake, |
timdennis | 3:b4435a45fdc7 | 217 | State_U_Motor_On, |
timdennis | 3:b4435a45fdc7 | 218 | State_U_Dwell_On, |
timdennis | 3:b4435a45fdc7 | 219 | State_U_Motor_Off, |
timdennis | 3:b4435a45fdc7 | 220 | State_U_Brake_On, |
timdennis | 3:b4435a45fdc7 | 221 | State_U_Dwell, |
timdennis | 3:b4435a45fdc7 | 222 | State_U_Write_USB, |
timdennis | 3:b4435a45fdc7 | 223 | State_U_Loop, |
timdennis | 3:b4435a45fdc7 | 224 | State_Timeout |
timdennis | 3:b4435a45fdc7 | 225 | } Lock_State; |
timdennis | 3:b4435a45fdc7 | 226 | |
timdennis | 3:b4435a45fdc7 | 227 | MSCFileSystem fs("fs"); //USB Mass Storage device file system |
timdennis | 3:b4435a45fdc7 | 228 | |
timdennis | 3:b4435a45fdc7 | 229 | char fileName[] = "/fs/Nodder.csv"; |
timdennis | 3:b4435a45fdc7 | 230 | |
chris | 2:c887cf1c3ed9 | 231 | |
chris | 2:c887cf1c3ed9 | 232 | int main() |
chris | 2:c887cf1c3ed9 | 233 | { |
timdennis | 3:b4435a45fdc7 | 234 | |
timdennis | 3:b4435a45fdc7 | 235 | // Essential actions on power-up, |
timdennis | 3:b4435a45fdc7 | 236 | // control MOSFET to prevent shoot-through |
timdennis | 3:b4435a45fdc7 | 237 | Brake_Off(); |
timdennis | 3:b4435a45fdc7 | 238 | Motor_Off(); |
timdennis | 3:b4435a45fdc7 | 239 | // switch off super bright LED (On when seeking Opto) |
timdennis | 3:b4435a45fdc7 | 240 | LED_TriColour = 7; |
timdennis | 3:b4435a45fdc7 | 241 | |
timdennis | 3:b4435a45fdc7 | 242 | set_time(0); // RTC used for elapsed time since power-up |
timdennis | 3:b4435a45fdc7 | 243 | |
timdennis | 3:b4435a45fdc7 | 244 | Init_LCD(); |
timdennis | 3:b4435a45fdc7 | 245 | Lock_State = State_L_Halt; |
timdennis | 3:b4435a45fdc7 | 246 | |
timdennis | 3:b4435a45fdc7 | 247 | // This message will persist until USB stick accessed... |
timdennis | 3:b4435a45fdc7 | 248 | lcd.cls(); |
timdennis | 3:b4435a45fdc7 | 249 | lcd.locate(0,0); |
timdennis | 3:b4435a45fdc7 | 250 | lcd.printf("Searching for....."); |
timdennis | 3:b4435a45fdc7 | 251 | lcd.locate(5,20); |
timdennis | 3:b4435a45fdc7 | 252 | lcd.printf("USB Stick!"); |
timdennis | 3:b4435a45fdc7 | 253 | |
timdennis | 3:b4435a45fdc7 | 254 | // On power-up (ie, power cycle), place a marker in log file |
timdennis | 3:b4435a45fdc7 | 255 | New_Log = 1; |
chris | 2:c887cf1c3ed9 | 256 | |
timdennis | 3:b4435a45fdc7 | 257 | // Check for existing log file on USB stick. |
timdennis | 3:b4435a45fdc7 | 258 | // if one exists, append to it. |
timdennis | 3:b4435a45fdc7 | 259 | // if it doesn't exist, create it with Header Info. |
timdennis | 3:b4435a45fdc7 | 260 | FILE * fp; |
timdennis | 3:b4435a45fdc7 | 261 | fp = fopen(fileName, "r" ); |
timdennis | 3:b4435a45fdc7 | 262 | if(fp == NULL) |
timdennis | 3:b4435a45fdc7 | 263 | { |
timdennis | 3:b4435a45fdc7 | 264 | FILE *fp = fopen(fileName,"w"); |
timdennis | 3:b4435a45fdc7 | 265 | fprintf(fp,"\n"); |
timdennis | 3:b4435a45fdc7 | 266 | fprintf(fp,"MBED Firmware Version: %s,,,,,,,------------,------------,------------,------------\n",FirmwareVersion); |
timdennis | 3:b4435a45fdc7 | 267 | fprintf(fp,",,,,,,,,,,\n"); |
timdennis | 3:b4435a45fdc7 | 268 | fprintf(fp,"----------------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------\n"); |
timdennis | 3:b4435a45fdc7 | 269 | fprintf(fp,"PWR ups: ,Power cycles marker,,,,, ,Power cycles ,,------->,=SUM(A:A)\n"); |
timdennis | 3:b4435a45fdc7 | 270 | fprintf(fp,"Cycles: ,Number of Lock/unlock cycles,,,,, ,Lock cycles ,,------->,=MAX(B:B)\n"); |
timdennis | 3:b4435a45fdc7 | 271 | fprintf(fp,"DegC: ,PCB Temperature [C]\n"); |
timdennis | 3:b4435a45fdc7 | 272 | fprintf(fp,"ms: ,Time spent traversing Opto Slot [ms]\n"); |
timdennis | 3:b4435a45fdc7 | 273 | fprintf(fp,"mA: ,Average current traversing Opto Slot [mA]\n"); |
timdennis | 3:b4435a45fdc7 | 274 | fprintf(fp,"\n"); |
timdennis | 3:b4435a45fdc7 | 275 | fprintf(fp,"\n"); |
timdennis | 3:b4435a45fdc7 | 276 | fprintf(fp,"\n"); |
timdennis | 3:b4435a45fdc7 | 277 | fprintf(fp,"\n"); |
timdennis | 3:b4435a45fdc7 | 278 | fprintf(fp,"----------------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------\n"); |
timdennis | 3:b4435a45fdc7 | 279 | |
timdennis | 3:b4435a45fdc7 | 280 | fprintf(fp, "PWR, Cycles, FW_Ver, Elapsed, Cycle_time, USB_Write_ms, PCB_C, Opto_Time_ms, Motor_Ave_mA, CAM_PWR_UP_DEGs, CAM_OptoStart_DEGs, CAM_OptoEnd_DEGs, CAM_OptoWindow_DEGs, CAM_LockMotorOff_DEGs, CAM_LockMotorRest_DEGs, CAM_LockRunOn_DEGs, CAM_UnLockMotorOff_DEGs, CAM_UnLockMotorRest_DEGs, CAM_UnLockRunOn_DEGs\n"); |
timdennis | 3:b4435a45fdc7 | 281 | fclose(fp); |
chris | 2:c887cf1c3ed9 | 282 | } |
chris | 2:c887cf1c3ed9 | 283 | |
timdennis | 3:b4435a45fdc7 | 284 | // Display message indicating that USB read is about to take place... |
timdennis | 3:b4435a45fdc7 | 285 | lcd.cls(); |
timdennis | 3:b4435a45fdc7 | 286 | lcd.locate(0,0); |
timdennis | 3:b4435a45fdc7 | 287 | lcd.printf("Reading..."); |
timdennis | 3:b4435a45fdc7 | 288 | lcd.locate(5,20); |
timdennis | 3:b4435a45fdc7 | 289 | lcd.printf("USB Stick"); |
timdennis | 3:b4435a45fdc7 | 290 | int ch; |
timdennis | 3:b4435a45fdc7 | 291 | int count=0; |
timdennis | 3:b4435a45fdc7 | 292 | |
timdennis | 3:b4435a45fdc7 | 293 | /* |
timdennis | 3:b4435a45fdc7 | 294 | do |
timdennis | 3:b4435a45fdc7 | 295 | { |
timdennis | 3:b4435a45fdc7 | 296 | ch = fgetc(fp); |
timdennis | 3:b4435a45fdc7 | 297 | if(ch == '\n') count++; |
timdennis | 3:b4435a45fdc7 | 298 | } |
timdennis | 3:b4435a45fdc7 | 299 | while( ch != EOF ); |
timdennis | 3:b4435a45fdc7 | 300 | */ |
timdennis | 3:b4435a45fdc7 | 301 | fp = fopen(fileName, "r" ); |
timdennis | 3:b4435a45fdc7 | 302 | rewind (fp); |
timdennis | 3:b4435a45fdc7 | 303 | fscanf (fp, "%d", &count); |
timdennis | 3:b4435a45fdc7 | 304 | fclose (fp); |
timdennis | 3:b4435a45fdc7 | 305 | cycle_counter = count+1; // Add 1 because it's incremented at end of state machine |
chris | 2:c887cf1c3ed9 | 306 | fclose(fp); |
timdennis | 3:b4435a45fdc7 | 307 | |
timdennis | 3:b4435a45fdc7 | 308 | /* |
timdennis | 3:b4435a45fdc7 | 309 | cycle_counter = count - HeaderRows; // subtract top 'header' rows |
timdennis | 3:b4435a45fdc7 | 310 | fclose(fp); |
timdennis | 3:b4435a45fdc7 | 311 | */ |
timdennis | 3:b4435a45fdc7 | 312 | |
timdennis | 3:b4435a45fdc7 | 313 | |
timdennis | 3:b4435a45fdc7 | 314 | iCam_Angle_Degrees_Power_Up = (int)(CAM_POT.read() * 360); |
timdennis | 3:b4435a45fdc7 | 315 | |
timdennis | 3:b4435a45fdc7 | 316 | // Commence Nodding Donkey at power up, joystick can stop/start... |
timdennis | 3:b4435a45fdc7 | 317 | Run_Status = 1; |
timdennis | 3:b4435a45fdc7 | 318 | |
timdennis | 3:b4435a45fdc7 | 319 | // Main 1 ms ticker used to execute state machine... |
timdennis | 3:b4435a45fdc7 | 320 | Lock_Ticker.attach(&Lock_Motor_Ticker, 0.001); |
timdennis | 3:b4435a45fdc7 | 321 | do |
timdennis | 3:b4435a45fdc7 | 322 | { |
timdennis | 3:b4435a45fdc7 | 323 | // Loop at Ticker frequency of 1 kHz... |
timdennis | 3:b4435a45fdc7 | 324 | // used for counting in milliseconds |
timdennis | 3:b4435a45fdc7 | 325 | if(ticker_counter != ticker_counter_p) |
timdennis | 3:b4435a45fdc7 | 326 | { |
timdennis | 3:b4435a45fdc7 | 327 | ticker_counter_p = ticker_counter; |
timdennis | 3:b4435a45fdc7 | 328 | switch (Lock_State) |
timdennis | 3:b4435a45fdc7 | 329 | { |
timdennis | 3:b4435a45fdc7 | 330 | case State_L_Halt: |
timdennis | 3:b4435a45fdc7 | 331 | sLCD_Text_1 = "Joy Down?"; |
timdennis | 3:b4435a45fdc7 | 332 | LED_Pattern = 0; // Start |
timdennis | 3:b4435a45fdc7 | 333 | Motor_Off(); |
timdennis | 3:b4435a45fdc7 | 334 | if(Run_Status == 1) |
timdennis | 3:b4435a45fdc7 | 335 | { |
timdennis | 3:b4435a45fdc7 | 336 | Lock_State = State_L_Init; |
timdennis | 3:b4435a45fdc7 | 337 | } |
timdennis | 3:b4435a45fdc7 | 338 | break; |
timdennis | 3:b4435a45fdc7 | 339 | // State_L_xxxxx refers to the Locking process |
timdennis | 3:b4435a45fdc7 | 340 | // State_U_xxxxx refers to the Unlocking process |
timdennis | 3:b4435a45fdc7 | 341 | case State_L_Init: |
timdennis | 3:b4435a45fdc7 | 342 | ticker_cycle_counter = 0; |
timdennis | 3:b4435a45fdc7 | 343 | Opto_To_Cam = 0; |
timdennis | 3:b4435a45fdc7 | 344 | sLCD_Text_1 = "Lock commence"; |
timdennis | 3:b4435a45fdc7 | 345 | LED_Pattern = 1; // Start |
timdennis | 3:b4435a45fdc7 | 346 | Lock_State = State_L_Hi_Thresh_Low; |
timdennis | 3:b4435a45fdc7 | 347 | break; |
timdennis | 3:b4435a45fdc7 | 348 | case State_L_Hi_Thresh_Low: |
timdennis | 3:b4435a45fdc7 | 349 | sLCD_Text_1 = "Current=LOW"; |
timdennis | 3:b4435a45fdc7 | 350 | HI_THRESHOLD_OUT = 1; |
timdennis | 3:b4435a45fdc7 | 351 | Lock_State = State_L_Release_Brake; |
timdennis | 3:b4435a45fdc7 | 352 | break; |
timdennis | 3:b4435a45fdc7 | 353 | case State_L_Release_Brake: |
timdennis | 3:b4435a45fdc7 | 354 | sLCD_Text_1 = "Brake Off"; |
timdennis | 3:b4435a45fdc7 | 355 | LED_Pattern = 3; // Locking |
timdennis | 3:b4435a45fdc7 | 356 | Brake_Off(); |
timdennis | 3:b4435a45fdc7 | 357 | Lock_State = State_L_Motor_On; |
timdennis | 3:b4435a45fdc7 | 358 | break; |
timdennis | 3:b4435a45fdc7 | 359 | case State_L_Motor_On: |
timdennis | 3:b4435a45fdc7 | 360 | sLCD_Text_1 = "Motor On"; |
timdennis | 3:b4435a45fdc7 | 361 | Motor_On(); |
timdennis | 3:b4435a45fdc7 | 362 | ticker_dwell = 0; |
timdennis | 3:b4435a45fdc7 | 363 | Lock_State = State_L_Ignore_Curr_For_time_100ms; |
timdennis | 3:b4435a45fdc7 | 364 | break; |
timdennis | 3:b4435a45fdc7 | 365 | case State_L_Ignore_Curr_For_time_100ms: |
timdennis | 3:b4435a45fdc7 | 366 | ticker_dwell++; |
timdennis | 3:b4435a45fdc7 | 367 | if(ticker_dwell > time_100ms) |
timdennis | 3:b4435a45fdc7 | 368 | { |
timdennis | 3:b4435a45fdc7 | 369 | ticker_dwell = 0; |
timdennis | 3:b4435a45fdc7 | 370 | Lock_State = State_L_Seek_Opto_Start; |
timdennis | 3:b4435a45fdc7 | 371 | } |
timdennis | 3:b4435a45fdc7 | 372 | // else if(Run_Status == 0) Lock_State = State_L_Halt; |
timdennis | 3:b4435a45fdc7 | 373 | break; |
timdennis | 3:b4435a45fdc7 | 374 | case State_L_Seek_Opto_Start: |
timdennis | 3:b4435a45fdc7 | 375 | ticker_dwell++; |
timdennis | 3:b4435a45fdc7 | 376 | if(ticker_dwell > time_5s) // timeout |
timdennis | 3:b4435a45fdc7 | 377 | { |
timdennis | 3:b4435a45fdc7 | 378 | sLCD_Text_1 = "Slot start??"; |
timdennis | 3:b4435a45fdc7 | 379 | Lock_State = State_Timeout; |
timdennis | 3:b4435a45fdc7 | 380 | } |
timdennis | 3:b4435a45fdc7 | 381 | else |
timdennis | 3:b4435a45fdc7 | 382 | { |
timdennis | 3:b4435a45fdc7 | 383 | LED_TriColour = 6; |
timdennis | 3:b4435a45fdc7 | 384 | sLCD_Text_1 = "Seek opto 1"; |
timdennis | 3:b4435a45fdc7 | 385 | if(OPTO_IN == 1) |
timdennis | 3:b4435a45fdc7 | 386 | { |
timdennis | 3:b4435a45fdc7 | 387 | // Start of cam, set 8V... |
timdennis | 3:b4435a45fdc7 | 388 | LED_TriColour = 7; |
timdennis | 3:b4435a45fdc7 | 389 | HI_THRESHOLD_OUT = 0; |
timdennis | 3:b4435a45fdc7 | 390 | ticker_dwell = 0; |
timdennis | 3:b4435a45fdc7 | 391 | Current_Raw = 0; |
timdennis | 3:b4435a45fdc7 | 392 | |
timdennis | 3:b4435a45fdc7 | 393 | iCam_Angle_Degrees_Opto_Start = (int)(CAM_POT.read() * 360); |
timdennis | 3:b4435a45fdc7 | 394 | // Snapshot of current as motor passes opto start... |
timdennis | 3:b4435a45fdc7 | 395 | Current_At_Opto_Start = I_MOTOR_ADC.read_u16(); |
timdennis | 3:b4435a45fdc7 | 396 | Current_At_Cam_Start_Target = Current_At_Opto_Start + Current_Delta; |
timdennis | 3:b4435a45fdc7 | 397 | |
timdennis | 3:b4435a45fdc7 | 398 | Lock_State = State_L_Seek_Opto_End; |
timdennis | 3:b4435a45fdc7 | 399 | } |
timdennis | 3:b4435a45fdc7 | 400 | // else if(Run_Status == 0) Lock_State = State_L_Halt; |
timdennis | 3:b4435a45fdc7 | 401 | } |
timdennis | 3:b4435a45fdc7 | 402 | break; |
timdennis | 3:b4435a45fdc7 | 403 | case State_L_Seek_Opto_End: |
timdennis | 3:b4435a45fdc7 | 404 | ticker_dwell++; |
timdennis | 3:b4435a45fdc7 | 405 | if(ticker_dwell > time_5s) // timeout |
timdennis | 3:b4435a45fdc7 | 406 | { |
timdennis | 3:b4435a45fdc7 | 407 | sLCD_Text_1 = "Slot end??"; |
timdennis | 3:b4435a45fdc7 | 408 | Lock_State = State_Timeout; |
timdennis | 3:b4435a45fdc7 | 409 | } |
timdennis | 3:b4435a45fdc7 | 410 | else |
timdennis | 3:b4435a45fdc7 | 411 | { |
timdennis | 3:b4435a45fdc7 | 412 | LED_TriColour = 5; |
timdennis | 3:b4435a45fdc7 | 413 | sLCD_Text_1 = "Seek opto 2"; |
timdennis | 3:b4435a45fdc7 | 414 | |
timdennis | 3:b4435a45fdc7 | 415 | |
timdennis | 3:b4435a45fdc7 | 416 | Current_At_Cam_Start_Captured = I_MOTOR_ADC.read_u16(); |
timdennis | 3:b4435a45fdc7 | 417 | |
timdennis | 3:b4435a45fdc7 | 418 | // Capture the number of milliseconds until Cam reached... |
timdennis | 3:b4435a45fdc7 | 419 | if(Current_At_Cam_Start_Captured > Current_At_Cam_Start_Target); |
timdennis | 3:b4435a45fdc7 | 420 | { |
timdennis | 3:b4435a45fdc7 | 421 | Opto_To_Cam = ticker_dwell; |
timdennis | 3:b4435a45fdc7 | 422 | } |
timdennis | 3:b4435a45fdc7 | 423 | |
timdennis | 3:b4435a45fdc7 | 424 | // sum the lockmotor current through opto window so that an average can be calculated... |
timdennis | 3:b4435a45fdc7 | 425 | Current_Raw = Current_Raw + I_MOTOR_ADC.read_u16(); |
timdennis | 3:b4435a45fdc7 | 426 | if(OPTO_IN == 0) // **** CHECK POLARITY ******** |
timdennis | 3:b4435a45fdc7 | 427 | { |
timdennis | 3:b4435a45fdc7 | 428 | // End of cam |
timdennis | 3:b4435a45fdc7 | 429 | LED_TriColour = 7; |
timdennis | 3:b4435a45fdc7 | 430 | |
timdennis | 3:b4435a45fdc7 | 431 | // Capture stats 'through' opto window |
timdennis | 3:b4435a45fdc7 | 432 | iCam_Angle_Degrees_Opto_End = (int)(CAM_POT.read() * 360); |
timdennis | 3:b4435a45fdc7 | 433 | iCam_Angle_Degrees_Window = iCam_Angle_Degrees_Opto_End - iCam_Angle_Degrees_Opto_Start; |
timdennis | 3:b4435a45fdc7 | 434 | Current_mA = (Current_Raw/ticker_dwell) / I_MOT_CONV; |
timdennis | 3:b4435a45fdc7 | 435 | Opto_Slot_ms = ticker_dwell; |
timdennis | 3:b4435a45fdc7 | 436 | ticker_dwell = 0; |
timdennis | 3:b4435a45fdc7 | 437 | Lock_State = State_L_Dwell_After_Opto_End; |
timdennis | 3:b4435a45fdc7 | 438 | } |
timdennis | 3:b4435a45fdc7 | 439 | // else if(Run_Status == 0) Lock_State = State_L_Halt; |
timdennis | 3:b4435a45fdc7 | 440 | } |
timdennis | 3:b4435a45fdc7 | 441 | break; |
timdennis | 3:b4435a45fdc7 | 442 | case State_L_Dwell_After_Opto_End: |
timdennis | 3:b4435a45fdc7 | 443 | // End of opto slot detected, continue for 25 ms |
timdennis | 3:b4435a45fdc7 | 444 | // as measured on PHC10 setup... |
timdennis | 3:b4435a45fdc7 | 445 | ticker_dwell++; |
timdennis | 3:b4435a45fdc7 | 446 | if(ticker_dwell > time_25ms) |
timdennis | 3:b4435a45fdc7 | 447 | { |
timdennis | 3:b4435a45fdc7 | 448 | // Flash LED to indicate ADC read taking place |
timdennis | 3:b4435a45fdc7 | 449 | LED_TriColour = 0; // Max |
timdennis | 3:b4435a45fdc7 | 450 | LED_TriColour = 7; // Off |
timdennis | 3:b4435a45fdc7 | 451 | |
timdennis | 3:b4435a45fdc7 | 452 | // End of cam, set 6V then brake. |
timdennis | 3:b4435a45fdc7 | 453 | HI_THRESHOLD_OUT = 1; |
timdennis | 3:b4435a45fdc7 | 454 | ticker_dwell = 0; |
timdennis | 3:b4435a45fdc7 | 455 | Lock_State = State_L_Motor_Off; |
timdennis | 3:b4435a45fdc7 | 456 | } |
timdennis | 3:b4435a45fdc7 | 457 | break; |
timdennis | 3:b4435a45fdc7 | 458 | case State_L_Motor_Off: |
timdennis | 3:b4435a45fdc7 | 459 | sLCD_Text_1 = "Motor off"; |
timdennis | 3:b4435a45fdc7 | 460 | Motor_Off(); |
timdennis | 3:b4435a45fdc7 | 461 | iCam_Angle_Degrees_Lock_Mot_Off = (int)(CAM_POT.read() * 360); |
timdennis | 3:b4435a45fdc7 | 462 | Lock_State = State_L_Brake_On; |
timdennis | 3:b4435a45fdc7 | 463 | break; |
timdennis | 3:b4435a45fdc7 | 464 | case State_L_Brake_On: |
timdennis | 3:b4435a45fdc7 | 465 | sLCD_Text_1 = "Brake on"; |
timdennis | 3:b4435a45fdc7 | 466 | LED_Pattern = 2; // Locked |
timdennis | 3:b4435a45fdc7 | 467 | Brake_On(); |
timdennis | 3:b4435a45fdc7 | 468 | ticker_dwell = 0; |
timdennis | 3:b4435a45fdc7 | 469 | Lock_State = State_L_Dwell_At_Locked; |
timdennis | 3:b4435a45fdc7 | 470 | break; |
timdennis | 3:b4435a45fdc7 | 471 | case State_L_Dwell_At_Locked: |
timdennis | 3:b4435a45fdc7 | 472 | ticker_dwell++; |
timdennis | 3:b4435a45fdc7 | 473 | LED_Pattern = 6; // Dwell at Locked |
timdennis | 3:b4435a45fdc7 | 474 | sLCD_Text_1 = "Locked"; |
timdennis | 3:b4435a45fdc7 | 475 | if(ticker_dwell > time_1s) |
timdennis | 3:b4435a45fdc7 | 476 | { |
timdennis | 3:b4435a45fdc7 | 477 | ticker_dwell = 0; |
timdennis | 3:b4435a45fdc7 | 478 | iCam_Angle_Degrees_Lock_Mot_Rest = (int)(CAM_POT.read() * 360); |
timdennis | 3:b4435a45fdc7 | 479 | iCam_Angle_Degrees_Lock_RunOn = iCam_Angle_Degrees_Lock_Mot_Rest - iCam_Angle_Degrees_Lock_Mot_Off; |
timdennis | 3:b4435a45fdc7 | 480 | Lock_State = State_U_Init; |
timdennis | 3:b4435a45fdc7 | 481 | } |
timdennis | 3:b4435a45fdc7 | 482 | else if(Run_Status == 0) Lock_State = State_L_Halt; |
timdennis | 3:b4435a45fdc7 | 483 | break; |
timdennis | 3:b4435a45fdc7 | 484 | case State_U_Init: |
timdennis | 3:b4435a45fdc7 | 485 | sLCD_Text_1 = "Unlock commence..."; |
timdennis | 3:b4435a45fdc7 | 486 | Lock_State = State_U_Hi_Thresh_Low; |
timdennis | 3:b4435a45fdc7 | 487 | break; |
timdennis | 3:b4435a45fdc7 | 488 | case State_U_Hi_Thresh_Low: |
timdennis | 3:b4435a45fdc7 | 489 | sLCD_Text_1 = "Current=HIGH"; |
timdennis | 3:b4435a45fdc7 | 490 | Lock_State = State_U_Release_Brake; |
timdennis | 3:b4435a45fdc7 | 491 | break; |
timdennis | 3:b4435a45fdc7 | 492 | case State_U_Release_Brake: |
timdennis | 3:b4435a45fdc7 | 493 | sLCD_Text_1 = "Brake Off"; |
timdennis | 3:b4435a45fdc7 | 494 | LED_Pattern = 4; // Unlocking |
timdennis | 3:b4435a45fdc7 | 495 | Brake_Off(); |
timdennis | 3:b4435a45fdc7 | 496 | Lock_State = State_U_Motor_On; |
timdennis | 3:b4435a45fdc7 | 497 | break; |
timdennis | 3:b4435a45fdc7 | 498 | case State_U_Motor_On: |
timdennis | 3:b4435a45fdc7 | 499 | sLCD_Text_1 = "Motor On"; |
timdennis | 3:b4435a45fdc7 | 500 | Motor_On(); |
timdennis | 3:b4435a45fdc7 | 501 | ticker_dwell = 0; |
timdennis | 3:b4435a45fdc7 | 502 | Lock_State = State_U_Dwell_On; |
timdennis | 3:b4435a45fdc7 | 503 | break; |
timdennis | 3:b4435a45fdc7 | 504 | case State_U_Dwell_On: |
timdennis | 3:b4435a45fdc7 | 505 | ticker_dwell++; |
timdennis | 3:b4435a45fdc7 | 506 | if(ticker_dwell > time_531ms) |
timdennis | 3:b4435a45fdc7 | 507 | { |
timdennis | 3:b4435a45fdc7 | 508 | ticker_dwell = 0; |
timdennis | 3:b4435a45fdc7 | 509 | Lock_State = State_U_Motor_Off; |
timdennis | 3:b4435a45fdc7 | 510 | } |
timdennis | 3:b4435a45fdc7 | 511 | // else if(Run_Status == 0) Lock_State = State_L_Halt; |
timdennis | 3:b4435a45fdc7 | 512 | break; |
timdennis | 3:b4435a45fdc7 | 513 | case State_U_Motor_Off: |
timdennis | 3:b4435a45fdc7 | 514 | sLCD_Text_1 = "Motor off"; |
timdennis | 3:b4435a45fdc7 | 515 | Motor_Off(); |
timdennis | 3:b4435a45fdc7 | 516 | iCam_Angle_Degrees_Unlocked_Mot_Off = (int)(CAM_POT.read() * 360); |
timdennis | 3:b4435a45fdc7 | 517 | Lock_State = State_U_Brake_On; |
timdennis | 3:b4435a45fdc7 | 518 | break; |
timdennis | 3:b4435a45fdc7 | 519 | case State_U_Brake_On: |
timdennis | 3:b4435a45fdc7 | 520 | sLCD_Text_1 = "Brake on"; |
timdennis | 3:b4435a45fdc7 | 521 | LED_Pattern = 2; // Unlocked |
timdennis | 3:b4435a45fdc7 | 522 | Brake_On(); |
timdennis | 3:b4435a45fdc7 | 523 | ticker_dwell = 0; |
timdennis | 3:b4435a45fdc7 | 524 | Lock_State = State_U_Dwell; |
timdennis | 3:b4435a45fdc7 | 525 | break; |
timdennis | 3:b4435a45fdc7 | 526 | case State_U_Dwell: // This dwell allows motor/cam to come to rest for measuring 'run-on' |
timdennis | 3:b4435a45fdc7 | 527 | LED_Pattern = 12; // Unlocked |
timdennis | 3:b4435a45fdc7 | 528 | ticker_dwell++; |
timdennis | 3:b4435a45fdc7 | 529 | if(ticker_dwell > time_500ms) |
timdennis | 3:b4435a45fdc7 | 530 | { |
timdennis | 3:b4435a45fdc7 | 531 | ticker_dwell = 0; |
timdennis | 3:b4435a45fdc7 | 532 | //LED_Pattern = 12; // Unlocked |
timdennis | 3:b4435a45fdc7 | 533 | // Flash LED to indicate ADC read taking place |
timdennis | 3:b4435a45fdc7 | 534 | LED_TriColour = 3; // Blue |
timdennis | 3:b4435a45fdc7 | 535 | iCam_Angle_Degrees_Unlocked_Rest = (int)(CAM_POT.read() * 360); |
timdennis | 3:b4435a45fdc7 | 536 | iCam_Angle_Degrees_Unlock_RunOn = iCam_Angle_Degrees_Unlocked_Rest - iCam_Angle_Degrees_Unlocked_Mot_Off; |
timdennis | 3:b4435a45fdc7 | 537 | LED_TriColour = 7; // Off |
timdennis | 3:b4435a45fdc7 | 538 | iPCB_Temp_C = (int)tmp.read(); // rounded off to integer value is good enough and keeps all data as ints! |
timdennis | 3:b4435a45fdc7 | 539 | Lock_State = State_U_Write_USB; |
timdennis | 3:b4435a45fdc7 | 540 | } |
timdennis | 3:b4435a45fdc7 | 541 | break; |
timdennis | 3:b4435a45fdc7 | 542 | |
timdennis | 3:b4435a45fdc7 | 543 | case State_U_Write_USB: |
timdennis | 3:b4435a45fdc7 | 544 | // Capture time taken to write to USB Stick (seems slow!) |
timdennis | 3:b4435a45fdc7 | 545 | iUSB_Start_Time = ticker_counter; |
timdennis | 3:b4435a45fdc7 | 546 | // Indicate writing to USB (see if it's a bottleneck!!) |
timdennis | 3:b4435a45fdc7 | 547 | LED_TriColour = 6; // Red |
timdennis | 3:b4435a45fdc7 | 548 | sLCD_Text_1 = "USB Write..."; |
timdennis | 3:b4435a45fdc7 | 549 | |
timdennis | 3:b4435a45fdc7 | 550 | //FILE *fp = fopen(fileName,"a"); |
timdennis | 3:b4435a45fdc7 | 551 | FILE *fp = fopen(fileName,"a"); // Allow repositioning of file pointer |
timdennis | 3:b4435a45fdc7 | 552 | // Place marker in log file to indicate a power cycle has occured |
timdennis | 3:b4435a45fdc7 | 553 | if(New_Log == 1) |
timdennis | 3:b4435a45fdc7 | 554 | { |
timdennis | 3:b4435a45fdc7 | 555 | New_Log = 0; |
timdennis | 3:b4435a45fdc7 | 556 | |
timdennis | 3:b4435a45fdc7 | 557 | fprintf(fp,"1,%d,%s,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",cycle_counter,FirmwareVersion,iSeconds_elapsed,iSeconds_per_cycle,iUSB_Write_ms,iPCB_Temp_C,Opto_Slot_ms,Current_mA,iCam_Angle_Degrees_Power_Up,iCam_Angle_Degrees_Opto_Start,iCam_Angle_Degrees_Opto_End,iCam_Angle_Degrees_Window,iCam_Angle_Degrees_Lock_Mot_Off,iCam_Angle_Degrees_Lock_Mot_Rest,iCam_Angle_Degrees_Lock_RunOn,iCam_Angle_Degrees_Unlocked_Mot_Off,iCam_Angle_Degrees_Unlocked_Rest,iCam_Angle_Degrees_Unlock_RunOn); |
timdennis | 3:b4435a45fdc7 | 558 | } |
timdennis | 3:b4435a45fdc7 | 559 | else |
timdennis | 3:b4435a45fdc7 | 560 | { |
timdennis | 3:b4435a45fdc7 | 561 | fprintf(fp,",%d,%s,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",cycle_counter,FirmwareVersion,iSeconds_elapsed,iSeconds_per_cycle,iUSB_Write_ms,iPCB_Temp_C,Opto_Slot_ms,Current_mA,iCam_Angle_Degrees_Power_Up,iCam_Angle_Degrees_Opto_Start,iCam_Angle_Degrees_Opto_End,iCam_Angle_Degrees_Window,iCam_Angle_Degrees_Lock_Mot_Off,iCam_Angle_Degrees_Lock_Mot_Rest,iCam_Angle_Degrees_Lock_RunOn,iCam_Angle_Degrees_Unlocked_Mot_Off,iCam_Angle_Degrees_Unlocked_Rest,iCam_Angle_Degrees_Unlock_RunOn); |
timdennis | 3:b4435a45fdc7 | 562 | } |
timdennis | 3:b4435a45fdc7 | 563 | fclose(fp); |
timdennis | 3:b4435a45fdc7 | 564 | // Store Cycle Count at start of file for quick retreival |
timdennis | 3:b4435a45fdc7 | 565 | fp = fopen(fileName,"r+"); |
timdennis | 3:b4435a45fdc7 | 566 | rewind (fp); |
timdennis | 3:b4435a45fdc7 | 567 | fprintf(fp,"%d",cycle_counter); |
timdennis | 3:b4435a45fdc7 | 568 | fclose(fp); |
timdennis | 3:b4435a45fdc7 | 569 | iSeconds_elapsed_p = iSeconds_elapsed; |
timdennis | 3:b4435a45fdc7 | 570 | |
timdennis | 3:b4435a45fdc7 | 571 | // End... Indicate writing to USB |
timdennis | 3:b4435a45fdc7 | 572 | LED_TriColour = 7; // Off |
timdennis | 3:b4435a45fdc7 | 573 | iUSB_End_Time = ticker_counter; |
timdennis | 3:b4435a45fdc7 | 574 | iUSB_Write_ms = iUSB_End_Time - iUSB_Start_Time; |
timdennis | 3:b4435a45fdc7 | 575 | Lock_State = State_U_Loop; |
timdennis | 3:b4435a45fdc7 | 576 | break; |
timdennis | 3:b4435a45fdc7 | 577 | |
timdennis | 3:b4435a45fdc7 | 578 | case State_U_Loop: |
timdennis | 3:b4435a45fdc7 | 579 | sLCD_Text_1 = "Unlocked"; |
timdennis | 3:b4435a45fdc7 | 580 | LED_Pattern = 8; // Unlocked |
timdennis | 3:b4435a45fdc7 | 581 | ticker_cycle_counter = 0; |
timdennis | 3:b4435a45fdc7 | 582 | ticker_counter = 0; |
timdennis | 3:b4435a45fdc7 | 583 | cycle_counter++; |
timdennis | 3:b4435a45fdc7 | 584 | cycle_counter_1s++; |
timdennis | 3:b4435a45fdc7 | 585 | tSeconds_elapsed = time(NULL); |
timdennis | 3:b4435a45fdc7 | 586 | iSeconds_elapsed = (int)tSeconds_elapsed; |
timdennis | 3:b4435a45fdc7 | 587 | iSeconds_per_cycle = iSeconds_elapsed - iSeconds_elapsed_p; |
timdennis | 3:b4435a45fdc7 | 588 | Lock_State = State_L_Init; |
timdennis | 3:b4435a45fdc7 | 589 | break; |
timdennis | 3:b4435a45fdc7 | 590 | |
timdennis | 3:b4435a45fdc7 | 591 | case State_Timeout: |
timdennis | 3:b4435a45fdc7 | 592 | LED_Pattern = 14; // Timeout Error |
timdennis | 3:b4435a45fdc7 | 593 | ticker_counter = 0; |
timdennis | 3:b4435a45fdc7 | 594 | Brake_Off(); |
timdennis | 3:b4435a45fdc7 | 595 | Motor_Off(); |
timdennis | 3:b4435a45fdc7 | 596 | break; |
timdennis | 3:b4435a45fdc7 | 597 | default: |
timdennis | 3:b4435a45fdc7 | 598 | sLCD_Text_1 = "Unknown ERROR"; |
timdennis | 3:b4435a45fdc7 | 599 | LED_Pattern = 15; // Unknown Error |
timdennis | 3:b4435a45fdc7 | 600 | Brake_Off(); |
timdennis | 3:b4435a45fdc7 | 601 | Motor_Off(); |
timdennis | 3:b4435a45fdc7 | 602 | //lcd.locate(0,20); |
timdennis | 3:b4435a45fdc7 | 603 | break; |
timdennis | 3:b4435a45fdc7 | 604 | } |
timdennis | 3:b4435a45fdc7 | 605 | } |
timdennis | 3:b4435a45fdc7 | 606 | |
timdennis | 3:b4435a45fdc7 | 607 | |
timdennis | 3:b4435a45fdc7 | 608 | if(LCD_update_time != LCD_update_time_p) |
timdennis | 3:b4435a45fdc7 | 609 | { |
timdennis | 3:b4435a45fdc7 | 610 | |
timdennis | 3:b4435a45fdc7 | 611 | LCD_update_time_p = LCD_update_time; |
timdennis | 3:b4435a45fdc7 | 612 | // 5 Hz update rate for LCD |
timdennis | 3:b4435a45fdc7 | 613 | |
timdennis | 3:b4435a45fdc7 | 614 | POT_tmp = (int)(CAM_POT.read() * 360); |
timdennis | 3:b4435a45fdc7 | 615 | lcd.cls(); |
timdennis | 3:b4435a45fdc7 | 616 | lcd.locate(0,0); |
timdennis | 3:b4435a45fdc7 | 617 | // Display number of cycles with 1000s formatting... |
timdennis | 3:b4435a45fdc7 | 618 | cycle_counter_k = cycle_counter / 1000; |
timdennis | 3:b4435a45fdc7 | 619 | cycle_counter_1s = cycle_counter % 1000; |
timdennis | 3:b4435a45fdc7 | 620 | lcd.printf("%dk%03d",cycle_counter_k,cycle_counter_1s); |
timdennis | 3:b4435a45fdc7 | 621 | lcd.locate(80,0); |
timdennis | 3:b4435a45fdc7 | 622 | lcd.printf("F/W: %s", FirmwareVersion); |
timdennis | 3:b4435a45fdc7 | 623 | |
timdennis | 3:b4435a45fdc7 | 624 | lcd.locate((POT_tmp/4),10); |
timdennis | 3:b4435a45fdc7 | 625 | lcd.printf("|||"); // 'graphical' display of angle |
timdennis | 3:b4435a45fdc7 | 626 | lcd.locate(10,20); |
timdennis | 3:b4435a45fdc7 | 627 | lcd.printf("||| |"); |
timdennis | 3:b4435a45fdc7 | 628 | } |
timdennis | 3:b4435a45fdc7 | 629 | } |
timdennis | 3:b4435a45fdc7 | 630 | while(1); |
timdennis | 3:b4435a45fdc7 | 631 | } |
timdennis | 3:b4435a45fdc7 | 632 | |
timdennis | 3:b4435a45fdc7 | 633 | |
timdennis | 3:b4435a45fdc7 | 634 | /* |
timdennis | 3:b4435a45fdc7 | 635 | 1 ms Ticker for main state machine execution |
timdennis | 3:b4435a45fdc7 | 636 | and slower 200 ms ticker for updating LCD... |
timdennis | 3:b4435a45fdc7 | 637 | */ |
timdennis | 3:b4435a45fdc7 | 638 | void Lock_Motor_Ticker(void) |
timdennis | 3:b4435a45fdc7 | 639 | { |
timdennis | 3:b4435a45fdc7 | 640 | ticker_counter++; |
timdennis | 3:b4435a45fdc7 | 641 | ticker_cycle_counter++; |
timdennis | 3:b4435a45fdc7 | 642 | LCD_update_counter++; |
timdennis | 3:b4435a45fdc7 | 643 | if(LCD_update_counter > 200) // |
timdennis | 3:b4435a45fdc7 | 644 | { |
timdennis | 3:b4435a45fdc7 | 645 | LCD_update_counter = 0; |
timdennis | 3:b4435a45fdc7 | 646 | LCD_update_time++; |
timdennis | 3:b4435a45fdc7 | 647 | } |
timdennis | 3:b4435a45fdc7 | 648 | |
timdennis | 3:b4435a45fdc7 | 649 | |
timdennis | 3:b4435a45fdc7 | 650 | |
timdennis | 3:b4435a45fdc7 | 651 | |
timdennis | 3:b4435a45fdc7 | 652 | |
timdennis | 3:b4435a45fdc7 | 653 | if(SWITCH_UP == 1) |
timdennis | 3:b4435a45fdc7 | 654 | { |
timdennis | 3:b4435a45fdc7 | 655 | Run_Status = 0; |
timdennis | 3:b4435a45fdc7 | 656 | } |
timdennis | 3:b4435a45fdc7 | 657 | else if(SWITCH_DN == 1) |
timdennis | 3:b4435a45fdc7 | 658 | { |
timdennis | 3:b4435a45fdc7 | 659 | Run_Status = 1; |
timdennis | 3:b4435a45fdc7 | 660 | } |
timdennis | 3:b4435a45fdc7 | 661 | } |
timdennis | 3:b4435a45fdc7 | 662 | |
timdennis | 3:b4435a45fdc7 | 663 | /* |
timdennis | 3:b4435a45fdc7 | 664 | Enable BRAKE only if Motor NOT running. |
timdennis | 3:b4435a45fdc7 | 665 | this is essential to avoid shoot through. |
timdennis | 3:b4435a45fdc7 | 666 | */ |
timdennis | 3:b4435a45fdc7 | 667 | void Brake_On(void) |
timdennis | 3:b4435a45fdc7 | 668 | { |
timdennis | 3:b4435a45fdc7 | 669 | if(RUN_OUT == 1) // 0 : MOTOR on |
timdennis | 3:b4435a45fdc7 | 670 | { |
timdennis | 3:b4435a45fdc7 | 671 | BRK_OUT = 0; // 0 : BRAKE On |
timdennis | 3:b4435a45fdc7 | 672 | } // 1 : BRAKE Off |
timdennis | 3:b4435a45fdc7 | 673 | else |
timdennis | 3:b4435a45fdc7 | 674 | { |
timdennis | 3:b4435a45fdc7 | 675 | RUN_OUT = 1; // Switch MOTOR off |
timdennis | 3:b4435a45fdc7 | 676 | wait_ms(1); // Allow MOSFET time to switch |
timdennis | 3:b4435a45fdc7 | 677 | BRK_OUT = 0; // Switch BRAKE on |
timdennis | 3:b4435a45fdc7 | 678 | } |
timdennis | 3:b4435a45fdc7 | 679 | } |
timdennis | 3:b4435a45fdc7 | 680 | |
timdennis | 3:b4435a45fdc7 | 681 | void Brake_Off(void) |
timdennis | 3:b4435a45fdc7 | 682 | { |
timdennis | 3:b4435a45fdc7 | 683 | BRK_OUT = 1; |
timdennis | 3:b4435a45fdc7 | 684 | } |
timdennis | 3:b4435a45fdc7 | 685 | |
timdennis | 3:b4435a45fdc7 | 686 | |
timdennis | 3:b4435a45fdc7 | 687 | /* |
timdennis | 3:b4435a45fdc7 | 688 | Enable MOTOR only if BRAKE Released. |
timdennis | 3:b4435a45fdc7 | 689 | this is essential to avoid shoot through. |
timdennis | 3:b4435a45fdc7 | 690 | */ |
timdennis | 3:b4435a45fdc7 | 691 | void Motor_On(void) |
timdennis | 3:b4435a45fdc7 | 692 | { |
timdennis | 3:b4435a45fdc7 | 693 | if(BRK_OUT == 1) // 0 : Brake On |
timdennis | 3:b4435a45fdc7 | 694 | { |
timdennis | 3:b4435a45fdc7 | 695 | RUN_OUT = 0; // 0 : motor on |
timdennis | 3:b4435a45fdc7 | 696 | } // 1 : motor off |
timdennis | 3:b4435a45fdc7 | 697 | else |
timdennis | 3:b4435a45fdc7 | 698 | { |
timdennis | 3:b4435a45fdc7 | 699 | BRK_OUT = 1; // Switch BRAKE off |
timdennis | 3:b4435a45fdc7 | 700 | wait_ms(1); // Allow MOSFET time to switch |
timdennis | 3:b4435a45fdc7 | 701 | RUN_OUT = 0; // Switch MOTOR on |
timdennis | 3:b4435a45fdc7 | 702 | } |
timdennis | 3:b4435a45fdc7 | 703 | } |
timdennis | 3:b4435a45fdc7 | 704 | |
timdennis | 3:b4435a45fdc7 | 705 | |
timdennis | 3:b4435a45fdc7 | 706 | void Motor_Off(void) |
timdennis | 3:b4435a45fdc7 | 707 | { |
timdennis | 3:b4435a45fdc7 | 708 | RUN_OUT = 1; |
timdennis | 3:b4435a45fdc7 | 709 | } |
timdennis | 3:b4435a45fdc7 | 710 | |
timdennis | 3:b4435a45fdc7 | 711 | |
timdennis | 3:b4435a45fdc7 | 712 | /* |
timdennis | 3:b4435a45fdc7 | 713 | Initial welcome message, |
timdennis | 3:b4435a45fdc7 | 714 | Setup font, |
timdennis | 3:b4435a45fdc7 | 715 | Test LEDs |
timdennis | 3:b4435a45fdc7 | 716 | */ |
timdennis | 3:b4435a45fdc7 | 717 | void Init_LCD(void) |
timdennis | 3:b4435a45fdc7 | 718 | { |
timdennis | 3:b4435a45fdc7 | 719 | lcd.set_font((unsigned char*) Arial_9); |
timdennis | 3:b4435a45fdc7 | 720 | |
timdennis | 3:b4435a45fdc7 | 721 | lcd.locate(0,0); |
timdennis | 3:b4435a45fdc7 | 722 | lcd.printf("PH10"); |
timdennis | 3:b4435a45fdc7 | 723 | lcd.locate(75,0); |
timdennis | 3:b4435a45fdc7 | 724 | lcd.printf("F/W: %s", FirmwareVersion); |
timdennis | 3:b4435a45fdc7 | 725 | LED_TriColour = 6; // Red |
timdennis | 3:b4435a45fdc7 | 726 | LED_Pattern = 12; |
timdennis | 3:b4435a45fdc7 | 727 | wait(0.8); |
timdennis | 3:b4435a45fdc7 | 728 | |
timdennis | 3:b4435a45fdc7 | 729 | lcd.locate(6,10); |
timdennis | 3:b4435a45fdc7 | 730 | lcd.printf("Nodding"); |
timdennis | 3:b4435a45fdc7 | 731 | lcd.locate(75,0); |
timdennis | 3:b4435a45fdc7 | 732 | lcd.printf("F/W: %s", FirmwareVersion); |
timdennis | 3:b4435a45fdc7 | 733 | LED_TriColour = 5; // Green |
timdennis | 3:b4435a45fdc7 | 734 | LED_Pattern = 6; |
timdennis | 3:b4435a45fdc7 | 735 | wait(0.8); |
timdennis | 3:b4435a45fdc7 | 736 | |
timdennis | 3:b4435a45fdc7 | 737 | lcd.locate(12,20); |
timdennis | 3:b4435a45fdc7 | 738 | lcd.printf("Donkey"); |
timdennis | 3:b4435a45fdc7 | 739 | lcd.locate(75,0); |
timdennis | 3:b4435a45fdc7 | 740 | lcd.printf("F/W: %s", FirmwareVersion); |
timdennis | 3:b4435a45fdc7 | 741 | LED_TriColour = 3; // Blue |
timdennis | 3:b4435a45fdc7 | 742 | LED_Pattern = 3; |
timdennis | 3:b4435a45fdc7 | 743 | wait(0.8); |
timdennis | 3:b4435a45fdc7 | 744 | LED_TriColour = 7; // Off |
timdennis | 3:b4435a45fdc7 | 745 | LED_Pattern = 0; // Off |
timdennis | 3:b4435a45fdc7 | 746 | lcd.cls(); |
timdennis | 3:b4435a45fdc7 | 747 | } |
timdennis | 3:b4435a45fdc7 | 748 |