Tim Dennis / Mbed 2 deprecated PH10_NoddingDonkey_Ver_1_0

First issue to MK

Dependencies:   mbed QEI LM75B C12832_lcd2 FatFileSystem MSCFileSystem

main.cpp@4:e3edfcdd50b5, 2019-01-23 (annotated)

Committer:
timdennis
Date:
Wed Jan 23 07:26:14 2019 +0000
Revision:
4:e3edfcdd50b5
Parent:
3:b4435a45fdc7 
First issue to MK

Who changed what in which revision?

UserRevisionLine numberNew 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