// print to the slave LCD
void slave_print(char *string)
{
serial_send_blocking("\xB8", 1);
char length = strlen(string);
serial_send_blocking(&length, 1); // send the string length
serial_send_blocking(string, length);
}
/**
* Transmit a tupel consiting of a channel ID and a sample value
* over the serial line to the client.
*
* param [in] channel channel ID (0..7)
* param [in] value sample value
*/
void xmit_tuple(int channel, int value)
{
unsigned char hi = (unsigned char)((value & (unsigned int)0x380) >> 7);
unsigned char lo = (unsigned char)(value & 0x7f);
serial_send_blocking((1 << 7) | (channel << 3) | (hi));
serial_send_blocking(lo);
}
void initialize()
{
play_from_program_space(welcome);
#ifdef DEBUG
// start receiving data at 9600 baud.
serial_set_baud_rate(9600);
serial_receive_ring(buffer, 100);
#endif
// initialize your QTR sensors
// unsigned char qtr_rc_pins[] = {IO_C0, IO_C1, IO_C2};
// unsigned char qtr_rc_pins[] = {IO_C0, IO_C1, IO_C2, IO_C3, IO_C4, IO_C5, IO_D7, IO_D4};
unsigned char qtr_rc_pins[] = {IO_D4, IO_D7, IO_C5, IO_C4, IO_C3, IO_C2, IO_C1, IO_C0};
qtr_rc_init(qtr_rc_pins, 8, 2000, IO_D2); // 800 us timeout, emitter pin PD2
#ifdef DEBUG
serial_send_blocking("Press Button A to start calibrating...\n", 39);
#endif
wait_for_button_press(BUTTON_A);
// Always wait for the button to be released so that the robot doesn't
// start moving until your hand is away from it.
wait_for_button_release(BUTTON_A);
delay_ms(800);
// then start calibration phase and move the sensors over both
// reflectance extremes they will encounter in your application:
// We use a value of 2000 for the timeout, which
// corresponds to 2000*0.4 us = 0.8 ms on our 20 MHz processor.
unsigned int counter; // used as a simple timer
for(counter = 0; counter < 82; counter++)
{
if(counter < 20 || counter >= 60)
set_motors(60,-60);
else
set_motors(-60,60);
qtr_calibrate(QTR_EMITTERS_ON);
// Since our counter runs to 80, the total delay will be
// 80*20 = 1600 ms.
delay_ms(20);
}
set_motors(0,0);
#ifdef DEBUG
serial_send_blocking("Press Button A to start line following...\n", 42);
#endif
wait_for_button_press(BUTTON_A);
wait_for_button_release(BUTTON_A);
}
void rob_serial_send_blocking_usb_comm (char * pBuffer, unsigned char size)
{
vTaskSuspendAll();
{
serial_send_blocking (USB_COMM, pBuffer, size);
}
xTaskResumeAll();
}
// sets up the pid constants on the 3pi for line following
void slave_set_pid(char max_speed, char p_num, char p_den, char d_num, char d_den)
{
char string[6] = "\xBB";
string[1] = max_speed;
string[2] = p_num;
string[3] = p_den;
string[4] = d_num;
string[5] = d_den;
serial_send_blocking(string,6);
}
void log_message(log_level_e lvl, char *fmt, ...) {
int msg_len;
char tmpbuf[LOG_BUFFER_SIZE];
va_list args;
va_start(args, fmt);
vsprintf(tmpbuf, fmt, args);
msg_len = strlen(tmpbuf);
va_end(args);
strncpy(g_serial_state.serbuf, tmpbuf, msg_len);
serial_send_blocking(USB_COMM, g_serial_state.serbuf, msg_len);
}
// Reads the line sensors and sends their values. This function can
// do either calibrated or uncalibrated readings. When doing calibrated readings,
// it only performs a new reading if we are not in PID mode. Otherwise, it sends
// the most recent result immediately.
void send_sensor_values(char calibrated)
{
if(calibrated)
{
if(!pid_enabled)
read_line_sensors_calibrated(sensors, IR_EMITTERS_ON);
}
else
read_line_sensors(sensors, IR_EMITTERS_ON);
serial_send_blocking((char *)sensors, 10);
}
// Computes the position of a black line using the read_line()
// function, and sends the value.
// Returns the last value computed if PID is running.
void send_line_position()
{
int message[1];
unsigned int tmp_sensors[5];
int line_position;
if(pid_enabled)
line_position = last_proportional+2000;
else line_position = read_line(tmp_sensors, IR_EMITTERS_ON);
message[0] = line_position;
serial_send_blocking((char *)message, 2);
}
// set the motor speeds
void slave_set_motors(int speed1, int speed2)
{
char message[4] = {0xC1, speed1, 0xC5, speed2};
if(speed1 < 0)
{
message[0] = 0xC2; // m1 backward
message[1] = -speed1;
}
if(speed2 < 0)
{
message[2] = 0xC6; // m2 backward
message[3] = -speed2;
}
serial_send_blocking(message,4);
}
// Runs through an automatic calibration sequence
void auto_calibrate()
{
time_reset();
set_motors(60, -60);
while(get_ms() < 250)
calibrate_line_sensors(IR_EMITTERS_ON);
set_motors(-60, 60);
while(get_ms() < 750)
calibrate_line_sensors(IR_EMITTERS_ON);
set_motors(60, -60);
while(get_ms() < 1000)
calibrate_line_sensors(IR_EMITTERS_ON);
set_motors(0, 0);
serial_send_blocking("c",1);
}
/*
Function will print out character array, the first length amount of characters, without adding and newlines or carriage returns.
*/
void print_to_serial(char *arr, int length)
{
serial_send_blocking(arr, length);
}
// clear the slave LCD
void slave_clear()
{
serial_send_blocking("\xB7",1);
}
// Sends the batter voltage in millivolts
void send_battery_millivolts()
{
int message[1];
message[0] = read_battery_millivolts();
serial_send_blocking((char *)message, 2);
}
// Sends the trimpot value, 0-1023.
void send_trimpot()
{
int message[1];
message[0] = read_trimpot();
serial_send_blocking((char *)message, 2);
}
// Sends the version of the slave code that is running.
// This function also shuts down the motors and disables PID, so it is
// useful as an initial command.
void send_signature()
{
serial_send_blocking("3pi1.1", 6);
set_motors(0,0);
pid_enabled = 0;
}
// stops the pid line following
void slave_stop_pid()
{
serial_send_blocking("\xBC", 1);
}
void print_new_line()
{
char arr[2] = {'\n','\r'};
serial_send_blocking(arr, 2);
}
// reset calibration
void slave_reset_calibration()
{
serial_send_blocking("\xB5",1);
}
int main()
{
char buffer[20];
// load the bar graph
load_custom_characters();
// configure serial clock for 115.2 kbaud
serial_set_baud_rate(115200);
// wait for the device to show up
while(1)
{
clear();
print("Master");
delay_ms(100);
serial_send("\x81",1);
if(serial_receive_blocking(buffer, 6, 50))
continue;
clear();
print("Connect");
lcd_goto_xy(0,1);
buffer[6] = 0;
print(buffer);
// clear the slave's LCD and display "Connect" and "OK" on two lines
// Put OK in the center to test x-y positioning
slave_clear();
slave_print("Connect");
slave_lcd_goto_xy(3,1);
slave_print("OK");
// play a tune
char tune[] = "\xB3 l16o6gab>c";
tune[1] = sizeof(tune)-3;
serial_send_blocking(tune,sizeof(tune)-1);
// wait
wait_for_button(ANY_BUTTON);
// reset calibration
slave_reset_calibration();
time_reset();
slave_auto_calibrate();
unsigned char speed1 = 0, speed2 = 0;
// read sensors in a loop
while(1)
{
serial_send("\x87",1); // returns calibrated sensor values
// read 10 characters
if(serial_receive_blocking(buffer, 10, 100))
break;
// get the line position
serial_send("\xB6", 1);
int line_position[1];
if(serial_receive_blocking((char *)line_position, 2, 100))
break;
// get the battery voltage
serial_send("\xB1",1);
// read 2 bytes
int battery_millivolts[1];
if(serial_receive_blocking((char *)battery_millivolts, 2, 100))
break;
// display readings
display_levels((unsigned int*)buffer);
lcd_goto_xy(5,0);
line_position[0] /= 4; // to get it into the range of 0-1000
if(line_position[0] == 1000)
line_position[0] = 999; // to keep it to a maximum of 3 characters
print_long(line_position[0]);
print(" ");
lcd_goto_xy(0,1);
print_long(battery_millivolts[0]);
print(" mV ");
delay_ms(10);
// if button A is pressed, increase motor1 speed
if(button_is_pressed(BUTTON_A) && speed1 < 127)
speed1 ++;
else if(speed1 > 1)
speed1 -= 2;
else if(speed1 > 0)
speed1 = 0;
// if button C is pressed, control motor2
if(button_is_pressed(BUTTON_C) && speed2 < 127)
speed2 ++;
else if(speed2 > 1)
speed2 -= 2;
else if(speed2 > 0)
speed2 = 0;
// if button B is pressed, do PID control
if(button_is_pressed(BUTTON_B))
slave_set_pid(40, 1, 20, 3, 2);
else
{
slave_stop_pid();
slave_set_motors(speed1, speed2);
}
}
}
while(1);
}
// go to coordinates x,y on the slave LCD
void slave_lcd_goto_xy(char x, char y)
{
serial_send_blocking("\xB9",1);
serial_send_blocking(&x,1);
serial_send_blocking(&y,1);
}
// calibrate (waits for a 1-byte response to indicate completion)
void slave_auto_calibrate()
{
int tmp_buffer[1];
serial_send_blocking("\xBA",1);
serial_receive_blocking((char *)tmp_buffer, 1, 10000);
}
