static rstatus_t
mcp_pre_run(struct context *ctx)
{
rstatus_t status;
struct opt *opt = &ctx->opt;
/* initialize logger */
status = log_init(opt->log_level, opt->log_filename);
if (status != MCP_OK) {
return status;
}
/* initialize buffer */
memset(ctx->buf1m, '0', sizeof(ctx->buf1m));
/* resolve server info */
status = mcp_resolve_addr(opt->server, opt->port, &opt->si);
if (status != MCP_OK) {
return status;
}
/*
* Initialize distribution for {conn, call, size} load generators with
* either default or user-supplied values.
*/
dist_init(&ctx->conn_dist, opt->conn_dopt.type, opt->conn_dopt.min,
opt->conn_dopt.max, opt->client.id);
dist_init(&ctx->call_dist, opt->call_dopt.type, opt->call_dopt.min,
opt->call_dopt.max, opt->client.id);
dist_init(&ctx->size_dist, opt->size_dopt.type, opt->size_dopt.min,
opt->size_dopt.max, opt->client.id);
/* initialize stats subsystem */
stats_init(ctx);
/* initialize timer */
timer_init();
/* initialize core */
status = core_init(ctx);
if (status != MCP_OK) {
return status;
}
return MCP_OK;
}
void init(){
led_init();
motor_init();
switch_init();
ground_init();
dist_init();
servo_init();
if(DEBUG) usart_init();
motor1 = Motor(&OCR1A, &MOTOR1_DIR_PORT, MOTOR1_DIR_PIN, MAX_POWER);
motor2 = Motor(&OCR1B, &MOTOR2_DIR_PORT, MOTOR2_DIR_PIN, MAX_POWER);
}
uint16_t dist_pingHQ( void ) {
dist_init();
for( uint16_t delay = 0; !bit8_and( xPIN(DIST_ECHO_LINE), DIST_ECHO ); delay++, _delay_us(1) ) {
if( delay > DIST_DELAY_MAX_HQ ) {
bit8_set( xDDR(DIST_ECHO_LINE), DIST_ECHO ); // set to output (low currently) to reset
return DIST_SENSE_HANG_UP;
}
} // wait for ECHO high; usually 430 loops (430 us)
for( uint16_t cnt = 0; cnt < DIST_DELAY_MAX_HQ; cnt++, _delay_us(6) ) {
if ( !bit8_and( xPIN(DIST_ECHO_LINE), DIST_ECHO ) ) { // wait for ECHO low
return cnt;
}
}
return DIST_SENSE_OUT_OF_RANGE;
}
mgasptr_t mgas_all_dmalloc(size_t size, size_t n_dims,
const size_t block_size[],
const size_t n_blocks[])
{
mgas_barrier();
size_t i;
size_t whole_size = 1;
for (i = 0; i < n_dims; i++) {
whole_size *= block_size[i];
whole_size *= n_blocks[i];
}
MGAS_CHECK(size == whole_size);
mgas_proc_t me = globals_get_pid();
gmt_t *gmt = globals_get_gmt();
// allocate addresses
mgasptr_t mp = MGAS_NULL;
if (me == 0) {
mp = gmt_alloc_dist(gmt, size);
MGAS_CHECK(mp != MGAS_NULL);
MGAS_ASSERT(mp >= MGASPTR_MIN);
}
comm_broadcast(&mp, sizeof(mgasptr_t), 0);
dist_t dist;
dist_init(&dist, n_dims, block_size, n_blocks);
gmt_validate_dist(gmt, mp, size, &dist);
mgas_barrier();
return mp;
}
/*### Mainloop ###*/
int main(void)
{
WDT_TRIGGERED(); //Watchdogreset?
init_sys();
init_pwm();
init_timer();
dist_init();
uart_init(UART_BAUD_SELECT(UART_MCU_BAUD_RATE,F_CPU)); //Bluetooth
comm_init();
init_adc();
mot.off = 0;
//The higher the task_i of the task is, the higher is the priority
MAIN_LED_OFF();
tasks[TASK_TIMER_ID].state = -1;
tasks[TASK_TIMER_ID].period = TASK_PERIOD_TIMER;
tasks[TASK_TIMER_ID].elapsedTime = 0;
tasks[TASK_TIMER_ID].running = 0;
tasks[TASK_TIMER_ID].task_fct = &task_timer;
tasks[TASK_SPEEDREG_ID].state = -1;
tasks[TASK_SPEEDREG_ID].period = TASK_PERIOD_SPEEDREG;
tasks[TASK_SPEEDREG_ID].elapsedTime = 0;
tasks[TASK_SPEEDREG_ID].running = 0;
tasks[TASK_SPEEDREG_ID].task_fct = &task_speedreg;
tasks[TASK_ANASENS_ID].state = -1;
tasks[TASK_ANASENS_ID].period = TASK_PERIOD_ANASENS;
tasks[TASK_ANASENS_ID].elapsedTime = 0;
tasks[TASK_ANASENS_ID].running = 0;
tasks[TASK_ANASENS_ID].task_fct = &task_anasens;
if(get_incrOk())
debug = 1;
else
debug = 0;
sei(); //Enable global interrupts. The Operating System and every task in it is running now and the cam already can regulate its initial aparture
bt_putStr_P(PSTR("\r\n\n\n\n\n\n\n\n"));
bt_putStr_P(PSTR("–––––––––––––––––––––––\r\n"));
bt_putStr_P(PSTR("| RIOS Scheduler v1.0 |\r\n"));
bt_putStr_P(PSTR("–––––––––––––––––––––––\r\n"));
bt_putStr_P(PSTR("rcj2015 v2.0\r\n"));
bt_putStr_P(PSTR("Subcontroller ATmega2560\r\n"));
bt_putStr_P(PSTR("Baud rate: "));bt_putLong(UART_COMM_BAUD_RATE); bt_putStr_P(PSTR(" Baud.\r\n"));
bt_putStr_P(PSTR("\r\n")); bt_putLong(timer); bt_putStr_P(PSTR(": System initialized, ")); bt_putLong(TASKS_NUM); bt_putStr_P(PSTR(" running tasks.\n\n"));
if(check_res)
{
motor_activate(0); //Shut down motor driver
if(debug > 0){bt_putStr_P(PSTR("\r\n")); bt_putLong(timer); bt_putStr(PSTR(": WARNING: RECOVERED AFTER AN UNEXPECTED SHUTDOWN!!!\n\n"));}
_delay_ms(5000);
}
wdt_enable(WDTO_1S); //activate watchdog
timer_get_tast = 120;
while(1)
{
wdt_reset();
////////////////////////////////////////////////////////////////////////////
comm_handler(); //Handle and respond to comm requests.
comm_reg_gateway();
////////////////////////////////////////////////////////////////////////////
if((timer_get_tast == 0) && (setup == 0))
{
timer_get_tast = -1;
mot.off = 0;
}
if(get_t1()) //Always reset...
{
mot.off = 1;
timer_get_tast = 120;
}
////////////////////LED etc...//////////////////////////////////////
if(timer_mainloop == 0)
{
if(check_res || fatal_err) //Flashing of the Info LED when there is an error
led_fault = 1;
else if(timer_nocomm == 0)
led_fault = 85; //Blink green if no communication/waiting for command
else if(mot.off)
led_fault = 30;
else
led_fault = 0;
if(fatal_err)
led_top = LED_TOP_FAT_ERR;
else
led_top = LED_TOP_NORMAL;
if(check_res)
{
if(!(debug_err_sendOneTime & (1<<0)))
{
if(debug > 1){bt_putStr_P(PSTR("\n\r")); bt_putLong(timer); bt_putStr_P(PSTR(": ERROR: RESET"));}
debug_err_sendOneTime |= (1<<0);
}
}
else debug_err_sendOneTime &= ~(1<<0);
//Batterie/Akku
if(batt_raw > 0)
{
batt_mV = (batt_raw*15)-300;
if(batt_mV < batt_mV_old)
{
batt_mV_old = batt_mV;
batt_percent = (0.037*batt_mV) - 363;
}
if(batt_percent < 20) //Batterie
led_heartbeatColor = batt_percent;
}
////////////////////////////////////////////////////////////////////////////
//LED heartbeat
if(rgb_led_mode == 0)
led_rgb(led_heartbeatColor, led_fault, led_top);
else
led_hsvToRgb(rgb_led_hue, rgb_led_sat, rgb_led_val);
timer_mainloop = TIMER_MAINLOOP;
}
}
return 0;
}
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);
}
}
