/*
!!!! This function is designed for the controller !!!!!
Purpose: To send the commands to each floor node as needed
*/
void Floor(unsigned char floor, unsigned char sub_comp, unsigned char instruction)
{
unsigned char data[DATA_LENGTH];
if(sub_comp == CALL_SWITCH1)
{
// Send led light so the user knows the car is coming
data[0] = floor;
data[1] = CALL_LED1;
data[2] = ON;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(floor, 0x00, 0x00, DATA_LENGTH, data);
switch(floor)
{
case F1:
LCDprintf("\nGo To: 1 \r",0);
break;
case F2:
LCDprintf("\nGo To: 2 \r",0);
break;
case F3:
LCDprintf("\nGo To: 3 \r",0);
break;
}
data[0] = PID;
data[1] = SET_POINT;
data[2] = floor;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(PID, 0x00, 0x00, DATA_LENGTH, data);
}
else if(sub_comp == CALL_SWITCH2)
{
data[0] = floor;
data[1] = CALL_LED2;
data[2] = ON;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(floor, 0x00, 0x00, DATA_LENGTH, data);
switch(floor)
{
case F1:
LCDprintf("\nGo To: 1 \r",0);
//dest_floor = F1;
break;
case F2:
LCDprintf("\nGo To: 2 \r",0);
break;
case F3:
LCDprintf("\nGo To: 3 \r",0);
break;
}
data[0] = PID;
data[1] = SET_POINT;
data[2] = floor;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(PID, 0x00, 0x00, DATA_LENGTH, data);
}
}
/*
!!!! This function is designed for the controller !!!!!
Purpose: Send out instructions from the elevator car node
Input: Requests from the car node
*/
void Car(unsigned char sub_comp, unsigned char instruction)
{
static unsigned char data[DATA_LENGTH];
switch(sub_comp)
{
// Floor button
case FLOOR_BUTTON:
// From the elevator car node to the disance sensor
/*data[0] = PID;
data[1] = SET_POINT;
data[2] = instruction;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(PID, 0x00, 0x00, DATA_LENGTH, data);*/
// From the car back to the car
data[0] = CAR;
data[1] = BUTTON_INDICATOR_ON;
data[2] = instruction;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(CAR, 0x00, 0x00, DATA_LENGTH, data);
switch(instruction)
{
case 3:
LCDprintf("\nGo To: 1 \r",0);
break;
case 4:
LCDprintf("\nGo To: 2 \r",0);
break;
case 5:
LCDprintf("\nGo To: 3 \r",0);
break;
}
break;
case 1:
data[0] = CAR;
data[1] = 1;
data[2] = instruction;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(CAR, 0x00, 0x00, DATA_LENGTH, data);
break;
// E-Stop
case E_STOP:
break;
}
}
/*
!!!! This function is designed for the controller !!!!!
Purpose: Parse the message into each of the components and then distrubute the commands as nessesary
Input: Recieve Queue
Output: none
*/
void Parse_Data(void)
{
// Local variables for assigning the bytes too
unsigned char node;
unsigned char sub_comp;
unsigned char instruction;
// Grab the three bytes and sort according to their representation
node = rxdata[0];
sub_comp = rxdata[1];
instruction = rxdata[2];
SCIprintf("%d%d%d", rxdata[0], rxdata[1], rxdata[2]);
//putsSCI(rxdata);
switch (node)
{
case PID:
Feedback(sub_comp, instruction);
break;
// Elevator Car
case CAR:
Car(sub_comp, instruction);
break;
// Floor 1
case F1:
Floor(F1, sub_comp, instruction);
break;
// Floor 2
case F2:
Floor(F2, sub_comp, instruction);
break;
// Floor 3
case F3:
Floor(F3, sub_comp, instruction);
break;
default:
msDELAY(100);
break;
}
}
/***********************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;
}
}
/*
!!!! This function is designed for the controller !!!!!
Purpose: Send out instructions from the PID
Input: sub_comp - should always be REC_POINT
instruction - should be the floor level
*/
void Feedback(unsigned char sub_comp, unsigned char instruction)
{
static unsigned char data[DATA_LENGTH];
if(sub_comp == REC_POINT) // Check to make sure that processing is correct
{
// Distribute the floor indicator to each node
data[0] = CAR;
data[1] = FLOOR_INDICATOR;
data[2] = instruction;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(CAR, 0x00, 0x00, DATA_LENGTH, data);
msDELAY(100);
data[0] = F1;
data[1] = FLOOR_INDICATOR;
data[2] = instruction;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(F1, 0x00, 0x00, DATA_LENGTH, data);
msDELAY(100);
data[0] = F2;
data[1] = FLOOR_INDICATOR;
data[2] = instruction;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(F2, 0x00, 0x00, DATA_LENGTH, data);
msDELAY(100);
data[0] = F3;
data[1] = FLOOR_INDICATOR;
data[2] = instruction;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(F3, 0x00, 0x00, DATA_LENGTH, data);
msDELAY(100);
//Shut off the call led
data[0] = instruction;
data[1] = CALL_LED1;
data[2] = OFF;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(instruction, 0x00, 0x00, DATA_LENGTH, data);
msDELAY(100);
data[0] = instruction;
data[1] = CALL_LED2;
data[2] = OFF;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(instruction, 0x00, 0x00, DATA_LENGTH, data);
msDELAY(100);
// Shut off elevator car led
data[0] = CAR;
data[1] = BUTTON_INDICATOR_OFF;
data[2] = instruction;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(CAR, 0x00, 0x00, DATA_LENGTH, data);
msDELAY(100);
// turn on the car door led
/* data[0] = CAR;
data[1] = DOOR;
data[2] = OPEN;
SCIprintf("%d%d%d", data[0], data[1], data[2]);
(void)CANSend(CAR, 0x00, 0x00, DATA_LENGTH, data); */
}
}