void exec_init() {
// initialize all sub-systems
hl_init();
schd_init();
lcd_init();
cam_init();
adc_init();
serv_init();
cont_init();
ui_init();
mot_init();
//comms_init();
// disable interrupt
disable_irq(EXEC_PIT_IRQ_NUM);
// enable the clock to the PIT module (pg. 299)
SIM_SCGC6 |= SIM_SCGC6_PIT_MASK;
// enable clock for PIT timers and allow timers to continue to
// run in debug mode (pg. 1038)
PIT_MCR = 0x0;
exec_disablePit();
exec_sysMode = EXEC_MODE_IDLE;
lcd_update();
exec_update();
}
/// \brief Executes an interactive command
/// \details Tries to match the beginning of the supplied string to the registered commands.
/// If a matching command string is found, its handler is invoked.
/// In case no suitable command could be matched, or the executed handler returns a
/// non-zero value, an error message is sent.
/// \param command
/// the full command line as a C-string
static inline void execute_command(const char * const command)
{
bool success = true;
if (strcmp(command, "clear") == 0) {
for (int i=0; i<80; ++i) {
usb_puts(PSTR(""));
}
}
else if (strncmp(command, "fwupdate", 8) == 0) {
success = exec_update(command);
}
else if (strcmp(command, "help") == 0) {
success = exec_help();
}
else if (strcmp(command, "reset") == 0) {
usb_puts(PSTR("Resetting device..."));
reset = true;
}
else {
// Iterate all user-defined commands and try to find a matching one
for (uint8_t i=0; i<user_commands_size; ++i) {
const struct serial_command* user_command = user_commands + i;
if (strncmp_P(command, user_command->cmd_string, strlen_P(user_command->cmd_string)) == 0) {
success = user_command->handler(command);
goto cleanup;
}
}
// No known command matches :-(
usb_printf(PSTR("Unknown command: [%s]" USB_NEWLINE), command);
usb_puts(PSTR("Type `help` for help." USB_NEWLINE));
}
cleanup:
if (!success) {
usb_printf(PSTR("Error executing command: [%s]" USB_NEWLINE), command);
}
// Clear command buffer and reset write pointer
memset(cmd_buffer, '\0', sizeof(cmd_buffer));
cmd_buffer_index = 0;
}
void cont_disProc() {
int ones = 0;
int tens = 0;
int hundreds = 0;
int error = 0;
int smoothing;
int averages[124];
int average;
int min_contrast = 40;
int difference = 0;
int total = 0;
int calcwidth = -1;
int leftedge = 0;
int rightedge = 0;
int center = -1;
int oldcenter = 50;
int linewidth = -1;
int i = 2;
int exp_line = 12;// Set expected line width
//int exp_ssline = 10; // Set expected SSLine width
int tolerance = 4; //set the expected tolerance
for (i=2; i<126; i++){
smoothing = (adc_rawCameraData[i+0]+adc_rawCameraData[i-1]+adc_rawCameraData[i-2]+adc_rawCameraData[i+1]+adc_rawCameraData[i+2])/5;
averages[i-2] = smoothing;
}
for (i=0; i<100; i++){
difference = averages[i+12]-averages[i+11];
derivative[i] = abs(difference);
total = total+derivative[i];
}
average = total/100;
cutoff = average*3; //////
if (cutoff < 4){
cutoff = 4;
}
for (i=0; i<100; i++){
if (derivative[i] > cutoff){
flag[i] = 1;
}
else{
flag[i] = 0;
}
}
for (i=3; i<96; i++){
if (flag[i] == 1 && leftedge ==0){
if(flag[i-1] == 1 || flag[i-2] ==1 || flag[i+1] == 1 || flag[i+2] ==1){
leftedge = i;
while (flag[i+1]==1|| flag[i+2] ==1 ){
if(flag[i-1] == 1 || flag[i-2] ==1 || flag[i+1] == 1 || flag[i+2] ==1|| flag[i+3] ==1 || flag[i-3] ==1 ){
leftedge = i;
i++;
}
}
}
}
else if (flag[i] == 1 && leftedge !=0 && rightedge ==0 && i> leftedge +4 ){
if(flag[i-1] == 1 || flag[i-2] ==1 || flag[i+1] == 1 || flag[i+2] ==1 ){
while ((flag[i+1]==1 || flag[i+2]==1)&& i<96){
if(flag[i-1] == 1 || flag[i-2] ==1 || flag[i+1] == 1 || flag[i+2] ==1 || flag[i+3] ==1 || flag[i-3] ==1 ){
rightedge = i;
i++;
}
}
}
}
if (leftedge != 0 && rightedge != 0){
calcwidth = rightedge - leftedge;
if (calcwidth < (exp_line +tolerance) && calcwidth > (exp_line -tolerance)){ //Is it the size of the line?
if (averages[leftedge+8]>(averages[leftedge+16]+min_contrast) && (averages[rightedge+8]+min_contrast)<averages[rightedge+16]){ // Is it a white to black transition?
center = ((leftedge + rightedge)/2)-1;
leftedge = rightedge;
rightedge = 0;
}
else if ((averages[leftedge+8]<(averages[leftedge+16]+min_contrast) && (averages[rightedge+8]+min_contrast)>averages[rightedge+16]&& (averages[rightedge+8]+min_contrast)>averages[rightedge+0])||(averages[leftedge+8]<(averages[leftedge+16]+min_contrast) && (averages[rightedge+8]+min_contrast)>averages[rightedge+16]&& (averages[rightedge+8]+min_contrast)>averages[rightedge+36])){
linewidth = calcwidth;
inframe = 1;
leftedge = rightedge;
rightedge = 0;
}
else { // Is it a white to black transition?
leftedge = rightedge; //We don't care, so set leftedge to be rightedge and keep looking
rightedge = 0;
}
}
else{
leftedge = rightedge;
rightedge = 0;
}
}
}
if (inframe == 1 && center != -1 && center > 25 && center < 75){
inframe = 0;
numcounts = 50;
}
else if (inframe == 0 && numcounts > 0 && center != -1 && center > 25 && center < 75){
numcounts--;
}
else if (inframe == 0 && numcounts == 0 && center != -1 && center > 25 && center < 75 && lapsremaining > 0){
lapsremaining--;
numcounts = -1;
}
// if (center == -1){
// center = oldcenter;
// }
// oldcenter = center;
cont_lineWidth = linewidth;
cont_lineLoc = center;
//
// ones=cont_lineLoc%10;
// tens=(cont_lineLoc/10)%10;
// hundreds=(cont_lineLoc/100)%10;
//
// //Update SLCD
// tlcd_set_numeric3(ones); // ones
// tlcd_set_numeric2(tens); // tens
// tlcd_set_numeric1(hundreds); // hundreds
if (cont_lineLoc >= 0){
error = cont_lineLoc-50;
correctionval = error+50;
//serv_angle = (correctionval * 2062) / 100;
}
else{
//serv_angle = oldservangle;
}
//oldservangle = serv_angle;
//serv_update();
if (center >= 0){
if (lostline > 0){
lostline--;
}
}
else if (center == -1 && lostline < 701){
lostline++;
}
// if (lapsremaining <= 0 || lostline >= 100){
// mot_motorSpeedA1 = 000;
// mot_motorSpeedA2 = 000;
//
// mot_motorSpeedB1 = 000;
// mot_motorSpeedB2 = 000;
//
// mot_update();
// }
//
// else if (serv_angle < 1100 && serv_angle > 962){
// mot_motorSpeedA1 = 0000;
// mot_motorSpeedA2 = maxspeed;
//
// mot_motorSpeedB1 = 0000;
// mot_motorSpeedB2 = maxspeed;
//
// mot_update();
// }
//
// else{
//
// if( serv_angle > 1100 ){
// mot_motorSpeedA1 = 0000;
// mot_motorSpeedA2 = maxspeed - (((serv_angle-1100)*2));
//
// mot_motorSpeedB1 = 0000;
// mot_motorSpeedB2 = maxspeed;
//
// mot_update();
// }
// else if (serv_angle < 962){
// mot_motorSpeedA1 = 0000;
// mot_motorSpeedA2 = maxspeed;
//
// mot_motorSpeedB1 = 0000;
// mot_motorSpeedB2 = maxspeed - (((962-serv_angle)*2));
//
// mot_update();
// }
//
//
// }
if (cont_lineLoc != -1) {
// motor setup
// mot_motorSpeedA1 = 0000;
// mot_motorSpeedA2 = 0000;
// mot_motorSpeedB1 = 0000;
// mot_motorSpeedB2 = 0000;
// mot_update();
exec_sysMode = EXEC_MODE_PERF_SPE;
exec_update();
}
cont_disServUpdateCnt++;
if (cont_disServUpdateCnt == 100) {
if (serv_angle < CONT_DIS_SERV_MAX) {
serv_angle += CONT_DIS_SERV_INC;
serv_update();
}
cont_disServUpdateCnt = 0;
}
}
