Tim Dennis
First issue to MK
Dependencies: mbed QEI LM75B C12832_lcd2 FatFileSystem MSCFileSystem
Diff: main.cpp
- Revision:
- 3:b4435a45fdc7
- Parent:
- 2:c887cf1c3ed9
--- a/main.cpp Tue Oct 16 15:47:35 2012 +0000
+++ b/main.cpp Wed Jan 23 07:23:10 2019 +0000
@@ -1,23 +1,748 @@
+/*
+--------------------------------------------------------------------------------
+CONFIDENTIAL
+Project no. 1368
+Author: Tim Dennis
+Copyright (c) 2016 Renishaw plc
+--------------------------------------------------------------------------------
+PH10 Lock Motor test rig (Nodding Donkey)
+A test rig to test the life of PH10 lock motor assemblies
+Initially, this rig will evaluate updated Faulhaber motors (9/9/2016)
+Info from Paul Boughey [pboughey@ems-ltd.com]…
+The new 1516 SR design is more powerful than the current 1616E and also has a
+number of modifications to its commutation system that should give a marked
+improvement in the lifetime compared to the current motor/gearbox assembly.
+--------------------------------------------------------------------------------
+Function...
+Upon Power up, the firmware will automatically commence cycling. This is to cater
+for power cuts, which could otherwise terminate a test, losing valuable test time.
+The MBED will log data to USB stick. Should a power cycle occur, the USB stick
+will enable the test to continue where it left off.
+--------------------------------------------------------------------------------
+Displayed Data on LCD...
+Firmware Version
+Firmware Status
+Lock/Unlock Cycles completed
+Time to transit Opto slot [ms]
+Average current whilst transitting Opto Slot [ms]
+--------------------------------------------------------------------------------
+Data logged to USB Stick...
+Firmware Version
+Power Cycles (A marked is placed in file to indicate power cycle occured)
+Lock/Unlock cycles completed (Target is 1 million)
+Time to transit Opto slot [ms]
+Average current whilst transitting Opto Slot [ms]
+--------------------------------------------------------------------------------
+Version history:
+1.0 Dev version used on Nodding Donkey A-1368-0700-01
+1.1 First used on Nodding Donkey A-1368-0700-02
+1.2 Added QEI code which 'hijacks' LED pins 2 and 4 on MBED
+1.3 Added support for analogue POT for transducing CAM Wheel position
+1.4 Amended Unlock state machine to 'BRAKE' after 531 ms (rather than run-on)
+1.5 Added power-up diaplay of pot voltage (BIST)
+1.6 Fixed POT angle measurment bug, removed hall sensor code
+1.7 Reduced time spent between cycles
+1.8 Changed cycle repeat time to 4 seconds
+1.9 Changed elapsed time recording to use MBED RTC (although without battery, it's volatile)
+1.10 Tidied up 'header rows'
+1.11 JM wants cycle time of 10s reducing/improving.
+ Also added FW_Ver to logfile (FW changes mid life test are therefore recorded)
+1.12 Changed 'Joystick Off' action so that motion ceases at Locked position only
+ Also changed PCB_Temp_C to 'integer' as float precision wasn't required.
+ Also moved left column of FW_Ver in log file
+1.13 Now store Cycle Count at top of Nodder.csv, rather than counting the rows
+ in Nodder.csv as this had become very slow on power-up.
+
+*/
+
#include "mbed.h"
+#include "C12832_lcd.h"
+#include "Small_7.h"
+#include "Arial_9.h"
+#include "stdio.h"
+#include <string>
#include "MSCFileSystem.h"
+#include "LM75B.h"
+
+#define FirmwareVersion "1.13"
+#define HeaderRows 15 // number of header rows in output file (before data starts)
+
+// From C-1368-0700-02-A, the following conversions were calculated...
+// Lockmotor: 4.864864 Volts per Amp
+// ADC FSD: 65535 == 3.3 V
+// 96612 ADC counts = 1000 mA
+// 1 ADC count = 0.0103507 mA
+
+#define I_MOT_CONV 96.612 // Motor Current [mA]
+#define V_REF_CONV 18.810 // V Ref [V]
+#define DEFLECTION_CONV 1.000 // Hall sensor
+
+#define time_25ms 25
+#define time_50ms 50
+#define time_100ms 100
+#define time_500ms 500
+#define time_531ms 531
+
+#define time_600ms 600
+#define time_1s 1000
+#define time_2s 3000
+#define time_4s 4000
+#define time_5s 5000
+#define time_6s 6000
+#define time_7s 7000
+#define time_8s 8000
+#define time_10s 10000
+
+C12832_LCD lcd; // LCD object
+
+// MBED I/O...
+DigitalIn OPTO_IN(p13);
+DigitalOut RUN_OUT(p9);
+DigitalOut BRK_OUT(p10);
+DigitalOut HI_THRESHOLD_OUT(p30);
+DigitalIn HI_LOCK_CUR_IN(p29);
+AnalogIn I_MOTOR_ADC(p17);
+AnalogIn V_REF_ADC(p16);
+AnalogIn V_DEFLECTION_ADC(p18); // Hall sensor to transduce deflection
+
+// MBED Joystick...
+DigitalIn SWITCH_UP(p15); // Stop Lock.Unlock operation
+DigitalIn SWITCH_DN(p12); // Resume operation
+
+// MBED Pot useful during debugging...
+AnalogIn Pot1(p19);
-MSCFileSystem fs("fs");
-DigitalOut led(LED1);
-AnalogIn pot1(p19);
+AnalogIn CAM_POT(p20); // Pot2 removed and Cam Angle Pot wired into here!
+
+// MBED LEDs...
+BusOut LED_Pattern(LED4,LED3,LED2,LED1);
+BusOut LED_TriColour(p23,p24,p25);
+
+LM75B tmp(p28,p27);
+
+int LED_TriColourCounter = 0;
+
+// Functions to control Motor to prevent MOSFET Shoot-through
+void Brake_On(void);
+void Brake_Off(void);
+void Motor_On(void);
+void Motor_Off(void);
+
+// Ticker ISR
+void Lock_Motor_Ticker(void);
+Ticker Lock_Ticker;
+
+void Init_LCD(void);
+
+int ticker_counter = 0;
+int ticker_counter_p = 0;
+int ticker_dwell = 0;
+
+int ticker_cycle_counter = 0;
+
+time_t tSeconds_elapsed;
+int iSeconds_elapsed;
+
+time_t tSeconds_elapsed_p;
+int iSeconds_elapsed_p;
+
+int iSeconds_per_cycle;
+int iUSB_Write_ms;
+int iUSB_Start_Time;
+int iUSB_End_Time;
+
+
+int cycle_counter = 0;
+int cycle_counter_1s = 0;
+int cycle_counter_k = 0;
+
+int LCD_update_time = 0;
+int LCD_update_time_p = 0;
+int LCD_update_counter = 0;
+
+int Current_Raw = 0;
+int Current_mA = 0;
+int Opto_Slot_ms = 0;
+
+int Current_At_Opto_Start = 0;
+int Current_At_Cam_Start_Target = 0;
+int Current_At_Cam_Start_Captured = 0;
+int Current_Delta = 4000; // An offset to capture current when cam starts
+
+int Opto_To_Cam = 0;
+
+int iPCB_Temp_C = 0;
+
+int vref_counter = 0;
+
+int POT_Loop;
+int POT_tmp;
+
+
+bool Run_Status = 0;
+bool New_Log = 0;
+
+int iCam_Angle_Degrees_Power_Up;
+int iCam_Angle_Degrees_Opto_Start;
+int iCam_Angle_Degrees_Opto_End;
+int iCam_Angle_Degrees_Window;
+int iCam_Angle_Degrees_Lock_Mot_Off;
+int iCam_Angle_Degrees_Lock_Mot_Rest;
+int iCam_Angle_Degrees_Lock_RunOn;
+int iCam_Angle_Degrees_Unlocked_Mot_Off;
+int iCam_Angle_Degrees_Unlocked_Rest;
+int iCam_Angle_Degrees_Unlock_RunOn;
+
+std::string sLCD_Text_1("ROW 1");
+std::string sLCD_Text_2("ROW 2");
+
+// State machine to control locking and unlocking...
+enum Lock_State_Type
+{
+ State_L_Halt,
+ State_L_Init,
+ State_L_Hi_Thresh_Low,
+ State_L_Release_Brake,
+ State_L_Motor_On,
+ State_L_Ignore_Curr_For_time_100ms,
+ State_L_Seek_Opto_Start,
+ State_L_Seek_Opto_End,
+ State_L_Dwell_After_Opto_End,
+ State_L_Motor_Off,
+ State_L_Brake_On,
+ State_L_Dwell_At_Locked,
+ State_U_Init,
+ State_U_Hi_Thresh_Low,
+ State_U_Release_Brake,
+ State_U_Motor_On,
+ State_U_Dwell_On,
+ State_U_Motor_Off,
+ State_U_Brake_On,
+ State_U_Dwell,
+ State_U_Write_USB,
+ State_U_Loop,
+ State_Timeout
+} Lock_State;
+
+MSCFileSystem fs("fs"); //USB Mass Storage device file system
+
+char fileName[] = "/fs/Nodder.csv";
+
int main()
{
- FILE *fp = fopen("/fs/test.csv","w");
- printf("Create filehandle for test.csv\n");
+
+// Essential actions on power-up,
+// control MOSFET to prevent shoot-through
+ Brake_Off();
+ Motor_Off();
+// switch off super bright LED (On when seeking Opto)
+ LED_TriColour = 7;
+
+ set_time(0); // RTC used for elapsed time since power-up
+
+ Init_LCD();
+ Lock_State = State_L_Halt;
+
+// This message will persist until USB stick accessed...
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Searching for.....");
+ lcd.locate(5,20);
+ lcd.printf("USB Stick!");
+
+// On power-up (ie, power cycle), place a marker in log file
+ New_Log = 1;
- printf("Writing to file\n");
- for (int i=0; i<100; i++) {
- fprintf(fp,"%.2f\n",pot1.read());
- wait(0.05);
- led=!led;
+// Check for existing log file on USB stick.
+// if one exists, append to it.
+// if it doesn't exist, create it with Header Info.
+ FILE * fp;
+ fp = fopen(fileName, "r" );
+ if(fp == NULL)
+ {
+ FILE *fp = fopen(fileName,"w");
+ fprintf(fp,"\n");
+ fprintf(fp,"MBED Firmware Version: %s,,,,,,,------------,------------,------------,------------\n",FirmwareVersion);
+ fprintf(fp,",,,,,,,,,,\n");
+ fprintf(fp,"----------------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------\n");
+ fprintf(fp,"PWR ups: ,Power cycles marker,,,,, ,Power cycles ,,------->,=SUM(A:A)\n");
+ fprintf(fp,"Cycles: ,Number of Lock/unlock cycles,,,,, ,Lock cycles ,,------->,=MAX(B:B)\n");
+ fprintf(fp,"DegC: ,PCB Temperature [C]\n");
+ fprintf(fp,"ms: ,Time spent traversing Opto Slot [ms]\n");
+ fprintf(fp,"mA: ,Average current traversing Opto Slot [mA]\n");
+ fprintf(fp,"\n");
+ fprintf(fp,"\n");
+ fprintf(fp,"\n");
+ fprintf(fp,"\n");
+ fprintf(fp,"----------------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------,------------\n");
+
+ 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");
+ fclose(fp);
}
+// Display message indicating that USB read is about to take place...
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Reading...");
+ lcd.locate(5,20);
+ lcd.printf("USB Stick");
+ int ch;
+ int count=0;
+
+ /*
+ do
+ {
+ ch = fgetc(fp);
+ if(ch == '\n') count++;
+ }
+ while( ch != EOF );
+ */
+ fp = fopen(fileName, "r" );
+ rewind (fp);
+ fscanf (fp, "%d", &count);
+ fclose (fp);
+ cycle_counter = count+1; // Add 1 because it's incremented at end of state machine
fclose(fp);
- printf("Close the handle\n");
- led=1;
-}
\ No newline at end of file
+
+ /*
+ cycle_counter = count - HeaderRows; // subtract top 'header' rows
+ fclose(fp);
+ */
+
+
+ iCam_Angle_Degrees_Power_Up = (int)(CAM_POT.read() * 360);
+
+// Commence Nodding Donkey at power up, joystick can stop/start...
+ Run_Status = 1;
+
+// Main 1 ms ticker used to execute state machine...
+ Lock_Ticker.attach(&Lock_Motor_Ticker, 0.001);
+ do
+ {
+// Loop at Ticker frequency of 1 kHz...
+// used for counting in milliseconds
+ if(ticker_counter != ticker_counter_p)
+ {
+ ticker_counter_p = ticker_counter;
+ switch (Lock_State)
+ {
+ case State_L_Halt:
+ sLCD_Text_1 = "Joy Down?";
+ LED_Pattern = 0; // Start
+ Motor_Off();
+ if(Run_Status == 1)
+ {
+ Lock_State = State_L_Init;
+ }
+ break;
+// State_L_xxxxx refers to the Locking process
+// State_U_xxxxx refers to the Unlocking process
+ case State_L_Init:
+ ticker_cycle_counter = 0;
+ Opto_To_Cam = 0;
+ sLCD_Text_1 = "Lock commence";
+ LED_Pattern = 1; // Start
+ Lock_State = State_L_Hi_Thresh_Low;
+ break;
+ case State_L_Hi_Thresh_Low:
+ sLCD_Text_1 = "Current=LOW";
+ HI_THRESHOLD_OUT = 1;
+ Lock_State = State_L_Release_Brake;
+ break;
+ case State_L_Release_Brake:
+ sLCD_Text_1 = "Brake Off";
+ LED_Pattern = 3; // Locking
+ Brake_Off();
+ Lock_State = State_L_Motor_On;
+ break;
+ case State_L_Motor_On:
+ sLCD_Text_1 = "Motor On";
+ Motor_On();
+ ticker_dwell = 0;
+ Lock_State = State_L_Ignore_Curr_For_time_100ms;
+ break;
+ case State_L_Ignore_Curr_For_time_100ms:
+ ticker_dwell++;
+ if(ticker_dwell > time_100ms)
+ {
+ ticker_dwell = 0;
+ Lock_State = State_L_Seek_Opto_Start;
+ }
+// else if(Run_Status == 0) Lock_State = State_L_Halt;
+ break;
+ case State_L_Seek_Opto_Start:
+ ticker_dwell++;
+ if(ticker_dwell > time_5s) // timeout
+ {
+ sLCD_Text_1 = "Slot start??";
+ Lock_State = State_Timeout;
+ }
+ else
+ {
+ LED_TriColour = 6;
+ sLCD_Text_1 = "Seek opto 1";
+ if(OPTO_IN == 1)
+ {
+// Start of cam, set 8V...
+ LED_TriColour = 7;
+ HI_THRESHOLD_OUT = 0;
+ ticker_dwell = 0;
+ Current_Raw = 0;
+
+ iCam_Angle_Degrees_Opto_Start = (int)(CAM_POT.read() * 360);
+ // Snapshot of current as motor passes opto start...
+ Current_At_Opto_Start = I_MOTOR_ADC.read_u16();
+ Current_At_Cam_Start_Target = Current_At_Opto_Start + Current_Delta;
+
+ Lock_State = State_L_Seek_Opto_End;
+ }
+// else if(Run_Status == 0) Lock_State = State_L_Halt;
+ }
+ break;
+ case State_L_Seek_Opto_End:
+ ticker_dwell++;
+ if(ticker_dwell > time_5s) // timeout
+ {
+ sLCD_Text_1 = "Slot end??";
+ Lock_State = State_Timeout;
+ }
+ else
+ {
+ LED_TriColour = 5;
+ sLCD_Text_1 = "Seek opto 2";
+
+
+ Current_At_Cam_Start_Captured = I_MOTOR_ADC.read_u16();
+
+ // Capture the number of milliseconds until Cam reached...
+ if(Current_At_Cam_Start_Captured > Current_At_Cam_Start_Target);
+ {
+ Opto_To_Cam = ticker_dwell;
+ }
+
+// sum the lockmotor current through opto window so that an average can be calculated...
+ Current_Raw = Current_Raw + I_MOTOR_ADC.read_u16();
+ if(OPTO_IN == 0) // **** CHECK POLARITY ********
+ {
+// End of cam
+ LED_TriColour = 7;
+
+// Capture stats 'through' opto window
+ iCam_Angle_Degrees_Opto_End = (int)(CAM_POT.read() * 360);
+ iCam_Angle_Degrees_Window = iCam_Angle_Degrees_Opto_End - iCam_Angle_Degrees_Opto_Start;
+ Current_mA = (Current_Raw/ticker_dwell) / I_MOT_CONV;
+ Opto_Slot_ms = ticker_dwell;
+ ticker_dwell = 0;
+ Lock_State = State_L_Dwell_After_Opto_End;
+ }
+// else if(Run_Status == 0) Lock_State = State_L_Halt;
+ }
+ break;
+ case State_L_Dwell_After_Opto_End:
+// End of opto slot detected, continue for 25 ms
+// as measured on PHC10 setup...
+ ticker_dwell++;
+ if(ticker_dwell > time_25ms)
+ {
+// Flash LED to indicate ADC read taking place
+ LED_TriColour = 0; // Max
+ LED_TriColour = 7; // Off
+
+// End of cam, set 6V then brake.
+ HI_THRESHOLD_OUT = 1;
+ ticker_dwell = 0;
+ Lock_State = State_L_Motor_Off;
+ }
+ break;
+ case State_L_Motor_Off:
+ sLCD_Text_1 = "Motor off";
+ Motor_Off();
+ iCam_Angle_Degrees_Lock_Mot_Off = (int)(CAM_POT.read() * 360);
+ Lock_State = State_L_Brake_On;
+ break;
+ case State_L_Brake_On:
+ sLCD_Text_1 = "Brake on";
+ LED_Pattern = 2; // Locked
+ Brake_On();
+ ticker_dwell = 0;
+ Lock_State = State_L_Dwell_At_Locked;
+ break;
+ case State_L_Dwell_At_Locked:
+ ticker_dwell++;
+ LED_Pattern = 6; // Dwell at Locked
+ sLCD_Text_1 = "Locked";
+ if(ticker_dwell > time_1s)
+ {
+ ticker_dwell = 0;
+ iCam_Angle_Degrees_Lock_Mot_Rest = (int)(CAM_POT.read() * 360);
+ iCam_Angle_Degrees_Lock_RunOn = iCam_Angle_Degrees_Lock_Mot_Rest - iCam_Angle_Degrees_Lock_Mot_Off;
+ Lock_State = State_U_Init;
+ }
+ else if(Run_Status == 0) Lock_State = State_L_Halt;
+ break;
+ case State_U_Init:
+ sLCD_Text_1 = "Unlock commence...";
+ Lock_State = State_U_Hi_Thresh_Low;
+ break;
+ case State_U_Hi_Thresh_Low:
+ sLCD_Text_1 = "Current=HIGH";
+ Lock_State = State_U_Release_Brake;
+ break;
+ case State_U_Release_Brake:
+ sLCD_Text_1 = "Brake Off";
+ LED_Pattern = 4; // Unlocking
+ Brake_Off();
+ Lock_State = State_U_Motor_On;
+ break;
+ case State_U_Motor_On:
+ sLCD_Text_1 = "Motor On";
+ Motor_On();
+ ticker_dwell = 0;
+ Lock_State = State_U_Dwell_On;
+ break;
+ case State_U_Dwell_On:
+ ticker_dwell++;
+ if(ticker_dwell > time_531ms)
+ {
+ ticker_dwell = 0;
+ Lock_State = State_U_Motor_Off;
+ }
+// else if(Run_Status == 0) Lock_State = State_L_Halt;
+ break;
+ case State_U_Motor_Off:
+ sLCD_Text_1 = "Motor off";
+ Motor_Off();
+ iCam_Angle_Degrees_Unlocked_Mot_Off = (int)(CAM_POT.read() * 360);
+ Lock_State = State_U_Brake_On;
+ break;
+ case State_U_Brake_On:
+ sLCD_Text_1 = "Brake on";
+ LED_Pattern = 2; // Unlocked
+ Brake_On();
+ ticker_dwell = 0;
+ Lock_State = State_U_Dwell;
+ break;
+ case State_U_Dwell: // This dwell allows motor/cam to come to rest for measuring 'run-on'
+ LED_Pattern = 12; // Unlocked
+ ticker_dwell++;
+ if(ticker_dwell > time_500ms)
+ {
+ ticker_dwell = 0;
+ //LED_Pattern = 12; // Unlocked
+ // Flash LED to indicate ADC read taking place
+ LED_TriColour = 3; // Blue
+ iCam_Angle_Degrees_Unlocked_Rest = (int)(CAM_POT.read() * 360);
+ iCam_Angle_Degrees_Unlock_RunOn = iCam_Angle_Degrees_Unlocked_Rest - iCam_Angle_Degrees_Unlocked_Mot_Off;
+ LED_TriColour = 7; // Off
+ iPCB_Temp_C = (int)tmp.read(); // rounded off to integer value is good enough and keeps all data as ints!
+ Lock_State = State_U_Write_USB;
+ }
+ break;
+
+ case State_U_Write_USB:
+ // Capture time taken to write to USB Stick (seems slow!)
+ iUSB_Start_Time = ticker_counter;
+ // Indicate writing to USB (see if it's a bottleneck!!)
+ LED_TriColour = 6; // Red
+ sLCD_Text_1 = "USB Write...";
+
+ //FILE *fp = fopen(fileName,"a");
+ FILE *fp = fopen(fileName,"a"); // Allow repositioning of file pointer
+// Place marker in log file to indicate a power cycle has occured
+ if(New_Log == 1)
+ {
+ New_Log = 0;
+
+ 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);
+ }
+ else
+ {
+ 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);
+ }
+ fclose(fp);
+// Store Cycle Count at start of file for quick retreival
+ fp = fopen(fileName,"r+");
+ rewind (fp);
+ fprintf(fp,"%d",cycle_counter);
+ fclose(fp);
+ iSeconds_elapsed_p = iSeconds_elapsed;
+
+ // End... Indicate writing to USB
+ LED_TriColour = 7; // Off
+ iUSB_End_Time = ticker_counter;
+ iUSB_Write_ms = iUSB_End_Time - iUSB_Start_Time;
+ Lock_State = State_U_Loop;
+ break;
+
+ case State_U_Loop:
+ sLCD_Text_1 = "Unlocked";
+ LED_Pattern = 8; // Unlocked
+ ticker_cycle_counter = 0;
+ ticker_counter = 0;
+ cycle_counter++;
+ cycle_counter_1s++;
+ tSeconds_elapsed = time(NULL);
+ iSeconds_elapsed = (int)tSeconds_elapsed;
+ iSeconds_per_cycle = iSeconds_elapsed - iSeconds_elapsed_p;
+ Lock_State = State_L_Init;
+ break;
+
+ case State_Timeout:
+ LED_Pattern = 14; // Timeout Error
+ ticker_counter = 0;
+ Brake_Off();
+ Motor_Off();
+ break;
+ default:
+ sLCD_Text_1 = "Unknown ERROR";
+ LED_Pattern = 15; // Unknown Error
+ Brake_Off();
+ Motor_Off();
+ //lcd.locate(0,20);
+ break;
+ }
+ }
+
+
+ if(LCD_update_time != LCD_update_time_p)
+ {
+
+ LCD_update_time_p = LCD_update_time;
+// 5 Hz update rate for LCD
+
+ POT_tmp = (int)(CAM_POT.read() * 360);
+ lcd.cls();
+ lcd.locate(0,0);
+ // Display number of cycles with 1000s formatting...
+ cycle_counter_k = cycle_counter / 1000;
+ cycle_counter_1s = cycle_counter % 1000;
+ lcd.printf("%dk%03d",cycle_counter_k,cycle_counter_1s);
+ lcd.locate(80,0);
+ lcd.printf("F/W: %s", FirmwareVersion);
+
+ lcd.locate((POT_tmp/4),10);
+ lcd.printf("|||"); // 'graphical' display of angle
+ lcd.locate(10,20);
+ lcd.printf("||| |");
+ }
+ }
+ while(1);
+}
+
+
+/*
+1 ms Ticker for main state machine execution
+and slower 200 ms ticker for updating LCD...
+*/
+void Lock_Motor_Ticker(void)
+{
+ ticker_counter++;
+ ticker_cycle_counter++;
+ LCD_update_counter++;
+ if(LCD_update_counter > 200) //
+ {
+ LCD_update_counter = 0;
+ LCD_update_time++;
+ }
+
+
+
+
+
+ if(SWITCH_UP == 1)
+ {
+ Run_Status = 0;
+ }
+ else if(SWITCH_DN == 1)
+ {
+ Run_Status = 1;
+ }
+}
+
+/*
+Enable BRAKE only if Motor NOT running.
+this is essential to avoid shoot through.
+*/
+void Brake_On(void)
+{
+ if(RUN_OUT == 1) // 0 : MOTOR on
+ {
+ BRK_OUT = 0; // 0 : BRAKE On
+ } // 1 : BRAKE Off
+ else
+ {
+ RUN_OUT = 1; // Switch MOTOR off
+ wait_ms(1); // Allow MOSFET time to switch
+ BRK_OUT = 0; // Switch BRAKE on
+ }
+}
+
+void Brake_Off(void)
+{
+ BRK_OUT = 1;
+}
+
+
+/*
+Enable MOTOR only if BRAKE Released.
+this is essential to avoid shoot through.
+*/
+void Motor_On(void)
+{
+ if(BRK_OUT == 1) // 0 : Brake On
+ {
+ RUN_OUT = 0; // 0 : motor on
+ } // 1 : motor off
+ else
+ {
+ BRK_OUT = 1; // Switch BRAKE off
+ wait_ms(1); // Allow MOSFET time to switch
+ RUN_OUT = 0; // Switch MOTOR on
+ }
+}
+
+
+void Motor_Off(void)
+{
+ RUN_OUT = 1;
+}
+
+
+/*
+Initial welcome message,
+Setup font,
+Test LEDs
+*/
+void Init_LCD(void)
+{
+ lcd.set_font((unsigned char*) Arial_9);
+
+ lcd.locate(0,0);
+ lcd.printf("PH10");
+ lcd.locate(75,0);
+ lcd.printf("F/W: %s", FirmwareVersion);
+ LED_TriColour = 6; // Red
+ LED_Pattern = 12;
+ wait(0.8);
+
+ lcd.locate(6,10);
+ lcd.printf("Nodding");
+ lcd.locate(75,0);
+ lcd.printf("F/W: %s", FirmwareVersion);
+ LED_TriColour = 5; // Green
+ LED_Pattern = 6;
+ wait(0.8);
+
+ lcd.locate(12,20);
+ lcd.printf("Donkey");
+ lcd.locate(75,0);
+ lcd.printf("F/W: %s", FirmwareVersion);
+ LED_TriColour = 3; // Blue
+ LED_Pattern = 3;
+ wait(0.8);
+ LED_TriColour = 7; // Off
+ LED_Pattern = 0; // Off
+ lcd.cls();
+}
+