void portb_isr(void){
if(SDA_CTRL & ISF){
SDA_CTRL |= ISF;
if (!(I2C0_S & I2C_S_BUSY)){
SDA_CTRL = (~IRQC_MASK & SDA_CTRL) | IRQC_NONE;
// We're done right here
} else {
if (++irqcount >= 2) {
SDA_CTRL = (~IRQC_MASK & SDA_CTRL) | IRQC_NONE;
}
}
}
if(MIN_LIMIT_CTRL & ISF){
if(parameters.homing & ENABLE_MIN){
stop_motion();
st.state = STATE_ERROR;
parameters.error_low = IMC_ERR_MECHANICAL;
}
MIN_LIMIT_CTRL |= ISF;
}
if(MAX_LIMIT_CTRL & ISF){
if(parameters.homing & ENABLE_MAX){
stop_motion();
st.state = STATE_ERROR;
parameters.error_low = IMC_ERR_MECHANICAL;
}
MAX_LIMIT_CTRL |= ISF;
}
if(SYNC_CTRL & ISF){
switch(st.state){
case STATE_SYNC:
{
uint32_t timer_value;
timer_value = SYNC_TIMEOUT - PIT_CVAL2; // Figure out how far we've gotten
PIT_TCTRL2 &= ~TEN; // Stop counting down
parameters.sync_error = timer_value;
}
execute_move();
break;
case STATE_IDLE:
if(queue_length() > 0){
execute_move();
break;
}
// Otherwise fall through and set an error condition
case STATE_EXECUTE:
// This should never happen - if we're in execute, we should hold the line low and not enable this interrupt...
st.state = STATE_ERROR;
parameters.error_low = IMC_ERR_TIMEOUT; // Maybe not best error code, but...
default:
; // Just handle this...
}
SYNC_CTRL = (SYNC_CTRL & ~IRQC_MASK) | IRQC_NONE;
SYNC_CTRL |= ISF;
}
}
void pit2_isr(void){
PIT_TFLG2 = 1;
// Stop the timer...
PIT_TCTRL2 &= ~TEN;
if(PIT_LDVAL2 != SYNC_DELAY){
// We've timed out and bad things are happening
st.state = STATE_ERROR;
parameters.error_low = IMC_ERR_TIMEOUT; // Maybe not best error code, but...
stop_motion();
}else{
CONTROL_DDR |= SYNC_BIT;
}
}
void pit2_isr(void){
PIT_TFLG2 = 1;
// Stop the timer...
PIT_TCTRL2 &= ~TEN;
if(PIT_LDVAL2 != SYNC_DELAY){
//||\\!! temp
//hid_printf("%u-%i pit2 timeout\n", get_systick_tenus(), (CONTROL_DDR & SYNC_BIT) ? -1 : CONTROL_PORT(DIR) & SYNC_BIT);
// We've timed out and bad things are happening
return;//||\\!! FIXME - For debugging I don't want a timeout error, so I am disabling this code!
st.state = STATE_ERROR;
parameters.error_low = IMC_ERR_TIMEOUT; // Maybe not best error code, but...
stop_motion();
}else{
CONTROL_DDR |= SYNC_BIT;
//||\\!! temp
//hid_printf("%u-%i Sync low\n",get_systick_tenus(), (CONTROL_DDR & SYNC_BIT) ? -1 : CONTROL_PORT(DIR) & SYNC_BIT);
}
}
/***********************cmdparser*******************************
*
* Purpose: Parse the command string to call the correct function.
*
* Input: char *cmdtype: input command string.
*
* Output: int result: Resulting integer value.
*
***************************************************************/
void cmdparser(char *buffer) {
char cmdtype[CMD_LEN+1] = {0};
int numchars = 0;
static int numcmd = 0; // Count of number of commands parsed
static byte tog = 0; // Laser toggle bit
char motor1_pct, motor2_pct;
cmdtype[0] = buffer[numchars];
cmdtype[1] = buffer[numchars+1];
cmdtype[2] = buffer[numchars+2];
cmdtype[CMD_LEN] = '\0'; // Terminate input command after three bytes, leaving just the command type
switch(cmdconv(cmdtype)) {
case 0: // If no command found, go to next character.
seekcmd(buffer, &numchars);
break;
case PNG: // ping
SCIprintf("png%05d",numcmd); // echo command confirmation with stamp.
//LCDclear(); LCDputs("Ping!");
numcmd++;
numchars += SCI_CMDSIZ;
break;
case ABT: // STOP THE PRESS!
SCIprintf("abt%05d",numcmd);
LCDclear(); LCDputs("Abort!\nAbort!");
stop_motion();
numcmd++;
numchars += SCI_CMDSIZ;
break;
case RES: // Resume operation
SCIprintf("res%05d",numcmd);
LCDclear(); LCDputs("Resuming...");
start_motion();
LCDclear(); LCDputs("Resumed");
numcmd++;
numchars += SCI_CMDSIZ;
break;
case MOV: // Set motor speed (0% - 100%)
SCIprintf("mov%05d", numcmd);
if(buffer[numchars+3] == '2') {
// Both motors selected
TC_INT_DISABLE(TC_MOTOR); // Disable motor control law
motor_set_speed(MOTOR1C, (char)atoi(&buffer[numchars+4]));
motor_set_speed(MOTOR2C, (char)atoi(&buffer[numchars+4]));
TC_INT_ENABLE(TC_MOTOR); // Re-enable motor control law
//LCDclear(); LCDprintf("\rM%c: %3d M%c: %3d", MOTOR1C, atoi(&buffer[numchars+4]), MOTOR2C, atoi(&buffer[numchars+4]));
}
else {
motor_set_speed(buffer[numchars+3], (char)atoi(&buffer[numchars+4]));
//LCDclear(); LCDprintf("\rMotor %c: %3d", buffer[numchars+3], atoi(&buffer[numchars+4]));
}
numcmd++;
numchars += SCI_CMDSIZ;
break;
case DST: // Set motor distance (+speed)
SCIprintf("dst%05d", numcmd);
switch(buffer[numchars+4]) {
case '0': // Setting a speed
motor1_pct = motor_convert(MOTOR1C, (int)atoi(&buffer[numchars+5]));
motor2_pct = motor_convert(MOTOR2C, (int)atoi(&buffer[numchars+5]));
// Set speed to both motors if 4th char is a '2'
if(buffer[numchars+3] == '2') {
TC_INT_DISABLE(TC_MOTOR); // Disable motor control law
motor_set_speed(MOTOR1C, motor1_pct);
motor_set_speed(MOTOR2C, motor2_pct);
TC_INT_ENABLE(TC_MOTOR); // Re-enable motor control law
} else
motor_set_speed(buffer[numchars+3],
motor_convert(buffer[numchars+3], (int)atoi(&buffer[numchars+5]))
);
//LCDclear(); LCDprintf("\rM1: %3d M2: %3d", motor1_pct, motor2_pct);
//LCDprintf("\nS1: %3d S2: %3d", atoi(&buffer[numchars+5]), atoi(&buffer[numchars+5]));
break;
case '1': // Setting a distance
// Set speed to both motors if 4th char is a '2'
if(buffer[numchars+3] == '2') {
motor_set_distance(MOTOR1C, (word)atoi(&buffer[numchars+5]));
motor_set_distance(MOTOR2C, (word)atoi(&buffer[numchars+5]));
//LCDclear(); LCDprintf("\nD%c: %3d D%c: %3d", MOTOR1C, atoi(&buffer[numchars+5]), MOTOR2C, atoi(&buffer[numchars+5]));
}
else {
motor_set_distance(buffer[numchars+3], (word)atoi(&buffer[numchars+5]));
//LCDclear(); LCDprintf("\rDist %c: %3d", buffer[numchars+3], atoi(&buffer[numchars+5]));
}
break;
}
numcmd++;
numchars += SCI_CMDSIZ;
break;
case SPN: // Spin in place
SCIprintf("spn%05d", numcmd);
DisableInterrupts;
motor_set_speed(MOTOR1C, -50);
motor_set_speed(MOTOR2C, 50);
motor_set_distance(MOTOR1C, (word)atoi(&buffer[numchars+3]));
motor_set_distance(MOTOR2C, (word)atoi(&buffer[numchars+3]));
EnableInterrupts;
SCIprintf("Dist: %3d\n", atoi(&buffer[numchars+3]));
numcmd++;
numchars += SCI_CMDSIZ;
break;
case AIM: // Toggle laser pointer
SCIprintf("aim%05d",numcmd);
tog = (tog) ? 0 : 1;
PTP_PTP0 = (tog) ? 1 : 0;
numcmd++;
numchars += SCI_CMDSIZ;
break;
case STP: // Force stop; set motor PWM to zero to stop the high frequency ringing!
SCIprintf("stp%05d",numcmd);
TC_INT_DISABLE(TC_MOTOR); // Disable motor control law
motor_set_duty(MOTOR1C, 0);
motor_set_duty(MOTOR2C, 0);
TC_INT_ENABLE(TC_MOTOR); // Re-enable motor control law
numcmd++;
numchars += SCI_CMDSIZ;
break;
}
}
