示例#1
0
/*
!!!! 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);	    
	}
}
示例#2
0
/*
!!!! 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;
	}
}
示例#3
0
/*
!!!! 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;
	}
}
示例#4
0
/***********************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;
    
    }
}
示例#5
0
/*
!!!! 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); */
			

	}
}