static void blink_led(void)
{
int i;
long long int t;
for(i=0;i<3;i++) {
leds_out_write(1);
t = clock_get_ms();
while(clock_get_ms() < t + 250);
leds_out_write(0);
t = clock_get_ms();
while(clock_get_ms() < t + 250);
}
}
static int check_test_mode(void)
{
char c;
long long int t;
t = clock_get_ms();
while(clock_get_ms() < t + 1000) {
if(readchar_nonblock()) {
c = readchar();
if((c == 't')||(c == 'T'))
return 1;
}
}
return 0;
}
int watchdog_expired(void)
{
int i;
long long int t;
t = 0x7fffffffffffffffLL;
for(i=0;i<MAX_WATCHDOGS;i++)
if(watchdogs[i].active && (watchdogs[i].threshold < t))
t = watchdogs[i].threshold;
return clock_get_ms() > t;
}
int watchdog_set(int ms)
{
int i, id;
id = -1;
for(i=0;i<MAX_WATCHDOGS;i++)
if(!watchdogs[i].active) {
id = i;
break;
}
if(id < 0) {
core_log("WARNING: Failed to add watchdog\n");
return id;
}
watchdogs[id].active = 1;
watchdogs[id].threshold = clock_get_ms() + ms;
return id;
}
static THD_FUNCTION(uart_thread, arg)
{
(void)arg;
chRegSetThreadName("UART thread");
// KPA.
event_listener_t kpa_event_listener;
chEvtRegisterMaskWithFlags((event_source_t*)chnGetEventSource(&SD1), &kpa_event_listener, EVENT_MASK(1), CHN_INPUT_AVAILABLE);
struct writer kpa_writer;
writer_init(&kpa_writer, &SD1, uart_write);
// FBV.
event_listener_t fbv_event_listener;
chEvtRegisterMaskWithFlags((event_source_t*)chnGetEventSource(&SD6), &fbv_event_listener, EVENT_MASK(2), CHN_INPUT_AVAILABLE);
struct writer fbv_writer;
writer_init(&fbv_writer, &SD6, uart_write);
// Bridge-
struct bridge bridge;
bridge_init(&bridge, &kpa_writer, &fbv_writer);
uint8_t byte = 0;
while (true)
{
eventflags_t evt = chEvtWaitAnyTimeout(EVENT_MASK(1) | EVENT_MASK(2), MS2ST(1));
if (evt & EVENT_MASK(1))
{
chEvtGetAndClearFlags(&kpa_event_listener);
while (readByte(&SD1, &byte))
bridge_update_kpa(&bridge, byte);
}
if (evt & EVENT_MASK(2))
{
chEvtGetAndClearFlags(&fbv_event_listener);
while (readByte(&SD6, &byte))
bridge_update_fbv(&bridge, byte);
}
bridge_update_time(&bridge, clock_get_ms());
}
}
void clock_test(void)
{
volatile short i = 0;
DDRB |= (1<<PB3) | (1<<PB4);
clock_set_ms(0);
while(1)
{
if( (clock_get_ms() % 500) == 0)
{
PORTB ^= (1<<PB4);
}
PORTB ^= (1<<PB3);
// wait so that the LED doesn't toggle twice
for(i = 0; i < 100;i++)
{
continue;
}
}
}
u32_t sys_now(void)
{
return clock_get_ms();
}
int main(void)
{
system_init();
clock_init();
led_init(); led_set(0x01); //show life
UART_Init(BAUD); UART_Write("\nInit"); //Show UART life
motor_init();
adc_init();
//Enable Analog pins
adc_enable(CHANNEL_SENSOR_LEFT);
adc_enable(CHANNEL_SENSOR_RIGHT);
adc_enable(CHANNEL_SENSOR_FRONT);
//Sensor value variables
uint16_t sensor_left_value = 0; uint16_t sensor_right_value = 0; uint16_t sensor_front_value = 0;
//Analog inputSignal conditioning arrays
circBuf_t left_buffer; circBuf_t right_buffer; circBuf_t front_buffer;
//Initialise sensor averaging buffers
initCircBuf(&left_buffer, ROLLING_AVERAGE_LENGTH);
initCircBuf(&right_buffer, ROLLING_AVERAGE_LENGTH);
initCircBuf(&front_buffer, ROLLING_AVERAGE_LENGTH);
//UART output buffer
char buffer[UART_BUFF_SIZE] = {0};
//=====Application specific variables===== //TODO: initialise circbuff
circBuf_t sweep_times;
initCircBuf(&sweep_times, SWEEP_TIME_MEMORY_LENGTH);
short sweep_del_t_last = 0;
short sweep_end_t_last = 0;
//time when front sensor begins to see grey.
uint32_t grey_time_start = 0;
bool sweep_ended = FALSE;
//set high if the front sensor crosses the line
bool front_crossed_black = FALSE;
//set high if front finds finish line
bool front_crossed_grey = FALSE;
bool sensor_update_serviced = TRUE;
action current_action = IDLE;
int16_t forward_speed = DEFAULT_FORWARD_SPEED;
int16_t turn_speed = DEFAULT_SPEED;
//Scheduler variables
uint32_t t = 0;
//Loop control time variables
uint32_t maze_logic_t_last = 0;
uint32_t sample_t_last = 0;
uint32_t UART_t_last = 0;
clock_set_ms(0);
sei(); // Enable all interrupts
UART_Write("ialized\n");
//wait for start command
DDRD &= ~BIT(7);
PORTD |= BIT(7);
//motor_set(128, 128);
while((PIND & BIT(7)))
{
continue;
}
while(1)
{
t = clock_get_ms();
//check if a sensor update has occured
if ((sensor_update_serviced == FALSE) &&
(t%MAZE_LOGIC_PERIOD == 0) && (t != maze_logic_t_last))
{
sensor_update_serviced = TRUE;
// finishing condition is a grey read for a set period
if(is_grey(sensor_front_value) && front_crossed_grey == FALSE)
{
front_crossed_grey = TRUE;
grey_time_start = t; //TODO: adjust so that finishing condition is a 1/2 whole sweeps on grey line
}
else if (is_grey(sensor_front_value) && front_crossed_grey == TRUE)
{
//
if ((grey_time_start + GREY_TIME) <= t )
{
// Finish line found. Stop robot.
maze_completed(); // wait for button push
front_crossed_grey = FALSE;
}
}
else
{
front_crossed_grey = FALSE;
}
//see if the front sensor crosses the line in case we run into a gap
if(is_black(sensor_front_value)&&front_crossed_black == FALSE)
{
front_crossed_black = TRUE;
//check for false finish line
if(front_crossed_grey)
front_crossed_grey = FALSE; //false alarm
}
// when both rear sensors go black, this indicates an intersection (turns included).
// try turning left
if(is_black(sensor_left_value) && is_black(sensor_right_value))
{
sweep_ended = TRUE;
motor_set(0, 255);
PORTB |= BIT(3);
PORTB |= BIT(4);
}
//when both sensors are completely white this indicates a dead end or a tape-gap
else if (is_white(sensor_left_value) && is_white(sensor_right_value))
{
sweep_ended = TRUE;
PORTB &= ~BIT(3);
PORTB &= ~BIT(4);
//current_action = ON_WHITE;
//Check if the front sensor is on black, or has been during the last sweep.
if(is_black(sensor_front_value) | front_crossed_black)
motor_set(255, 255);
else if (is_white(sensor_front_value))
motor_set(-255, 255);
}
//sweep to the side that reads the darkest value
else if (sensor_left_value + SENSOR_TOLLERANCE < sensor_right_value)
{
PORTB &= ~BIT(3);
PORTB |= BIT(4);
if (current_action == SWEEP_LEFT)
sweep_ended = TRUE;
current_action = SWEEP_RIGHT;
motor_set(forward_speed + turn_speed, forward_speed);
}
else if(sensor_right_value + SENSOR_TOLLERANCE< sensor_left_value)
{
PORTB |= BIT(3);
PORTB &= ~BIT(4);
if (current_action == SWEEP_RIGHT)
sweep_ended = TRUE;
current_action = SWEEP_LEFT;
motor_set(forward_speed, forward_speed+ turn_speed);
}
//If a new sweep started this cycle, find how long it took
if (sweep_ended)
{
//reset front black crossing detection variable
sweep_ended = FALSE;
if (front_crossed_black)
front_crossed_black = FALSE;
//Calculate sweep time
sweep_del_t_last = t - sweep_end_t_last;
sweep_end_t_last = t;
writeCircBuf(&sweep_times, sweep_del_t_last);
//adjust turn_speed for battery level.
if (sweep_del_t_last > IDEAL_SWEEP_TIME)
{
turn_speed += 5;
}
if (sweep_del_t_last < IDEAL_SWEEP_TIME)
{
turn_speed -= 5;
}
turn_speed = regulate_within(turn_speed, MIN_TURN_SPEED, MAX_TURN_SPEED);
}
}
//Sensor value update
if((t%SAMPLE_PERIOD == 0) & (t!=sample_t_last))
{
sample_t_last = t;
//read in analog values
sensor_update(CHANNEL_SENSOR_LEFT, &left_buffer, &sensor_left_value );
sensor_update(CHANNEL_SENSOR_RIGHT, &right_buffer, &sensor_right_value );
sensor_update(CHANNEL_SENSOR_FRONT, &front_buffer, &sensor_front_value );
sensor_update_serviced = FALSE;
}
//display debug information
if((t%UART_PERIOD == 0) & (t != UART_t_last) & UART_ENABLED)
{
UART_t_last = t;
sprintf(buffer, "sweep_time: %u \n", sweep_del_t_last);
UART_Write(buffer);
sprintf(buffer, "L: %u F: %u R: %u", sensor_left_value, sensor_front_value, sensor_right_value);
UART_Write(buffer);
UART_Write("\n");
}
}
}
