First issue to MK
Dependencies: mbed QEI LM75B C12832_lcd2 FatFileSystem MSCFileSystem
main.cpp
- Committer:
- timdennis
- Date:
- 2019-01-23
- Revision:
- 4:e3edfcdd50b5
- Parent:
- 3:b4435a45fdc7
File content as of revision 4:e3edfcdd50b5:
/* -------------------------------------------------------------------------------- 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); 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() { // 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; // 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); /* 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(); }