void main(void) {
char buffer[SCI_BUFSIZ+1] = {0};
// Initialize all nessesary modules
timer_init();
SCIinit();
encoder_init();
motor_init();
msleep(16); LCDinit();
//start_heartbeat(); // Not used, TCNT overflow interrupts causing issues
DDRP |= PTP_PTP0_MASK; // Set DDR for laser GPIO
// Motors off initially
motor_set_speed(MOTOR1C, 0);
motor_set_speed(MOTOR2C, 0);
EnableInterrupts;
LCDclear(); LCDputs("Ready.");
SCIputs("HCS12 ready to go!");
for(EVER) {
SCIdequeue(buffer);
cmdparser(buffer);
memset(buffer, 0, SCI_BUFSIZ+1); // Clear out the command buffer after each command parsed
//LCDclear(); LCDprintf("\r%7d %7d\n%7d %7d", drive_value1, drive_value2, speed_error1, speed_error2);
}
}
int main(void)
{
unsigned char buffer[17]; // Where we are reading to
buffer[16] = '\0'; // Terminating string
int page = 0xa0;
int offset = 0;
init_timer0(); // LCD needs timer0 to function
LCDinit(); // Initializing LCD
ini_i2c(); // Initializing i2c
/******************** MAIN LOOP *********************/
while(1) {
offset &= 0xff;
buffer[0] = offset;
escreve_i2c(page, buffer, 1); /* tell i2c we want to read from page + offset */
if(le_i2c(page, buffer, 16)) { /* If we failed to read 16 bytes */
LCDclear();
LCDgoto(0, 0);
LCDputs("Problema de leitura");
LCDgoto(1, 0);
LCDputs(buffer);
} else {
LCDgoto(0, 0); // Go to first line, first column
LCDputs("Pg = ");
LCDputchar(page + '0'); // Prints page number as char
LCDgoto(0, 8); // Go to the second half of first line
LCDputs("Of = ");
LCDputchar(offset + '0'); // Prints offset as char
LCDgoto(1, 0); // Fisrt column of the second line
LCDputs(buffer); // Prints buffer
}
}
return 0;
}
BYTE U0get (int mtempo)
{
BYTE d;
int idx;
static int brilho=0x38;
int k,seg;
int tempo, tini;
tini = Timer;
tempo = mtempo / 1000;
/* Wait while Rx buffer is empty */
seg = RTC_SEC;
k=10000;
while (!RxFifo0.count) {
idx = LEBOTAO;
if((mtempo > 0) && (Timer-tini) >= mtempo) return ' ';
switch(idx){
case SW3: brilho += 2; // KB4 incrementa contraste
case SW4: brilho --; // KB5 decrementa contraste
break;
case SW2: if(k==0) return ' ';
break;
case SW1: tempo=5;
break;
default: if(k) k--; break;
}
if(tempo > 0 && seg!=RTC_SEC) {
LCDcomando(0x86);
LCDputchar(tempo+'0');
LCDputchar(' ');
LCDcomando(0x87);
LCDputs(" T=");
if((--tempo) == 0) return ' ';
seg = RTC_SEC;
}
}
U0IER = 0; /* Disable interrupts */
idx = RxFifo0.rptr;
d = RxFifo0.buff[idx]; /* Get a byte from Rx buffer */
RxFifo0.rptr = (idx + 1) % BUFFER_SIZE;
RxFifo0.count--;
U0IER = 0x07; /* Enable interrupt */
return d;
}
void main(void) {
__delay_ms(100); // Wait for things to settle
// Setting up Button Input
ANSELB = 0; // Digital Input
// LCD initilization
LCDinit();
TRISBbits.RB5 = 1; // RB5 is button input
WPUBbits.WPUB5 = 1; // Weak pullup enabled for B5
__delay_ms(100); // Wait for things to settle
while(!PORTBbits.RB5) // Wait until B5 goes low
{
continue;
}
LCDputs("Ruth"); // Add Ruth to line 1
char *message = "Hello World - April 2016"; // Store message for line 2 in a variable
while(1)
{
for(int i = 0; i < 9; i++) // For loop to add to the message pointer
{
LCDwriteLineTwo((message+i)); // Print message starting at incremented pointer up to \0
__delay_ms(1000); // Delay cannot be much longer than a second due to program limits
__delay_ms(1000);
__delay_ms(250);
}
__delay_ms(1000); // Wait 1 extra second before beginning again
}
return;
}
void callbox(byte my_floor) {
byte rxmessage[PAYLOAD_SIZE]; // Received data payload
static byte floor, direction;
word distance;
static byte flag_dist_init = 0;
CANframe txframe; // Transmitted CAN frame
floor = 0xFF; // Start at false floor
direction = DIRECTION_STATIONARY; // Assume starting car direction is stationary
if (!flag_dist_init){
dist_init();
flag_dist_init = 1;
}
if(SW1 && !sw1_pressed) {
sw1_pressed = 1;
button_up(my_floor);
}
if(SW2 && !sw2_pressed) {
sw2_pressed = 1;
button_down(my_floor);
}
if(!SW1) sw1_pressed = 0;
if(!SW2) sw2_pressed = 0;
runSerialCAN(MSCAN_NODE_ID);
if(data_available()) {
CANget(rxmessage);
switch(rxmessage[0]) {
case CMD_LOCATION:
floor = rxmessage[1];
direction = rxmessage[2];
#ifdef USE_LCD
LCDclear();
LCDprintf("Floor: %d\nDir: %d", floor, direction);
#endif
#ifdef USE_LED7
led7_write(led7_table[floor]);
#endif
break;
case CMD_BUTTON_CALL:
rxmessage[1] == DIRECTION_UP ? button_up(my_floor) : button_down(my_floor);
break;
case CMD_ERROR:
#ifdef USE_LCD
LCDclear();
LCDprintf("Error condition\nreceived!");
#endif
break;
default:
#ifdef USE_LCD
LCDclear();
LCDputs("Unknown command");
#endif
break;
}
// Turn off indicator LED once car has reached local floor
if(floor == my_floor) {
LED1 = 0; LED2 = 0;
}
}
// Sonar sensor currently attached to callbox 1
// Send off distance message to controller node
if (my_floor == FLOOR1) {
distance = dist_read();
txframe.id = MSCAN_CTL_ID;
txframe.priority = 0x01;
txframe.length = 3;
txframe.payload[0] = CMD_DISTANCE;
txframe.payload[1] = (distance & 0xFF00) >> 8;
txframe.payload[2] = (distance & 0x00FF);
CANsend(&txframe);
}
/*
* Controller functionality
* - Send elevator location messages to callboxes
* - Listen for button press messages
*/
void controller() {
byte sw1_pressed = 0, sw2_pressed = 0;
byte rxmessage[PAYLOAD_SIZE]; // Received data payload
byte button_pressed;
byte next_floor;
char *button_floor_str, *button_direction_str;
byte update_lcd = 1;
byte cycle_count = 0;
word distance; // car height in cm, distance measurement in mm
byte cur_floor;
byte last_floor = 0;
//byte b; // used for debug manual frame sending testing
dist_init();
mctrl_init(); // Initialize servo motor controller
for(;;) {
mctrl_update();
cycle_count++;
if ( cycle_count == 10 ) {
update_lcd = 1;
cycle_count = 0;
}
if ( update_lcd ) {
update_lcd = 0;
/*
#ifdef USE_LCD
if ( cur_floor == 0 ) {
LCDclear();
LCDputs("No car");
} else {
LCDclear();
LCDprintf("%dmm/F%d", car_height, cur_floor);
}
#endif
*/
}
// CAN bus <-> serial link
// Check for new incoming messages and send out received messages via serial
runSerialCAN(MSCAN_NODE_ID);
/*
while ( sci_bytesAvailable() ) {
sci_readByte(&b);
lcd_putc(b);
} */
if(data_available()) {
CANget(rxmessage);
switch(rxmessage[0]) {
case CMD_BUTTON_CALL:
button_pressed = rxmessage[1];
addToQueue(button_pressed);
next_floor = peekNextFloor();
pid_setpoint(FLOOR2SETPOINT(next_floor));
switch(cur_floor) {
case FLOOR1:
button_floor_str = "1";
break;
case FLOOR2:
button_floor_str = "2";
break;
case FLOOR3:
button_floor_str = "3";
break;
default:
break;
}
if(next_floor == cur_floor) {
button_direction_str = "stat";
} else if(next_floor > cur_floor) {
button_direction_str = "up ";
} else {
button_direction_str = "down";
}
#ifdef USE_LCD
LCDhome();
LCDprintf("\nFloor%s Dir %s", button_floor_str, button_direction_str);
#else
#ifdef USE_LCD2
lcd_goto(0x10); // Start at second line
lcd_puts("Floor");
lcd_puts(button_floor_str);
lcd_puts(" Dir ");
lcd_puts(button_direction_str);
#endif
#endif
break;
case CMD_BUTTON_CAR:
button_pressed = rxmessage[1];
if(button_pressed == BUTTON_STOP) {
// call emergency stop function
} else if(button_pressed < BUTTON_DOOR_CLOSE) {
addToQueue(button_pressed);
}
next_floor = peekNextFloor();
pid_setpoint(FLOOR2SETPOINT(next_floor));
switch(cur_floor) {
case FLOOR1:
button_floor_str = "1";
break;
case FLOOR2:
button_floor_str = "2";
break;
case FLOOR3:
button_floor_str = "3";
break;
default:
break;
}
if(next_floor == cur_floor){
button_direction_str = "stat";
}else if(next_floor > cur_floor){
button_direction_str = "up ";
}else {
button_direction_str = "down";
}
#ifdef USE_LCD
LCDhome();
LCDprintf("\nFloor%s Dir %s", button_floor_str, button_direction_str);
#endif
#ifdef USE_LCD2
lcd_goto(0x10); // Start at second line
lcd_puts("Floor");
lcd_puts(button_floor_str);
lcd_puts(" Dir ");
lcd_puts(button_direction_str);
#endif
break;
case CMD_DISTANCE:
distance = (rxmessage[1] << 8) | rxmessage[2];
pid_feedback(distance);
if (distance < SETPOINT_F1 + FLOOR_MARGIN) cur_floor = FLOOR1;
if (distance > SETPOINT_F2 - FLOOR_MARGIN && distance < SETPOINT_F2 + FLOOR_MARGIN) cur_floor = FLOOR2;
if (distance > SETPOINT_F3 - FLOOR_MARGIN && distance < SETPOINT_F3 + FLOOR_MARGIN) cur_floor = FLOOR3;
if ( distance > 1500 ) {
cur_floor = 0;
#ifdef USE_LED7
led7_write(led7_bars[1]);
#endif
} else {
#ifdef USE_LED7
led7_write(led7_table[cur_floor]);
#endif
if ( cur_floor != last_floor ) {
update_floor(cur_floor);
last_floor = cur_floor;
// if we have reached the target floor, pop off the top of the queue
// TODO: change name of getNextFloor() to be more descriptive
if(cur_floor == next_floor){
getNextFloor();
next_floor = peekNextFloor();
pid_setpoint(FLOOR2SETPOINT(next_floor));
}
}
}
#ifdef USE_LCD
LCDhome();
LCDprintf("Dist: %4d", distance);
#endif
break;
case CMD_ERROR:
break;
default:
#ifdef USE_LCD
LCDclear();
LCDputs("\nUnknown command");
#endif
#ifdef USE_LCD2
lcd_goto(0x10); // Start at second line
lcd_puts("Unknown command");
#endif
break;
}
}
delay_ms(100);
}
}
/***********************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;
}
}
