/***************************************************************
Return the current state of all parts of robot
***************************************************************/
void return_all()
{
//printf(" in return_all_sensors() ");
start_checksum();
push_char(0x50);
push_char(0xAF);
push_char(0x81);
push_word(get_sonar(1));
push_word(get_sonar(2));
push_word(get_sonar(3));
push_word(get_line_follower(1));
push_word(get_line_follower(2));
push_word(get_line_follower(3));
push_word(get_gyro());
push_byte(get_bumper(1)); # check new spec
push_byte(get_bumper(2));
push_byte(get_motor(1));
push_byte(get_motor(2));
push_byte(get_motor(3));
send_checksum();
// Now 3 + 6 + 6 + 2 + 2 + 3 + 1 = 23
}
void send_msg(uint8_t addr, uint8_t cmd, const volatile __memx void *data, uint16_t datalen)
{
const volatile __memx uint8_t *data_u8 = data;
send_msg_header(addr, cmd, datalen);
for (size_t i = 0; i < datalen; ++i) {
buffer_send(data_u8[i], true);
}
send_checksum();
}
/***************************************************************
Set the state of single motor
***************************************************************/
void set_single_motor(int id, int power)
{
set_motor(id, _dec(power));
start_checksum();
push_char(0x50);
push_char(0xAF);
push_char(0x01);
push_char(0x10 + id);
send_checksum();
}
/***************************************************************
Set the current state of drive motors
***************************************************************/
void set_drive_motors(int power1, int power2)
{
set_motor(1, _dec(power1));
set_motor(2, _dec(power2));
start_checksum();
push_char(0x50);
push_char(0xAF);
push_char(0x01);
push_char(0x60);
send_checksum();
}
/***************************************************************
Set the forward speed and turn rate
***************************************************************/
void set_speed_and_turn(int spd, int turn)
{
int speed = _dec(spd);
int turnrate = _dec(turn);
set_motor(1, 0);
set_motor(2, 0);
start_checksum();
push_char(0x50);
push_char(0xAF);
push_char(0x01);
push_char(0x61);
send_checksum();
}
/***************************************************************
Return the current state of all sensors
***************************************************************/
void return_all_sensors()
{
//printf(" in return_all_sensors() ");
start_checksum();
push_char(0x50);
push_char(0xAF);
push_char(0x80);
// fake data for now
push_byte(get_sonar(1));
push_byte(get_sonar(2));
push_byte(get_sonar(3));
push_byte(get_line_follower(1));
push_byte(get_line_follower(2));
push_byte(get_line_follower(3));
push_byte(get_gyro());
push_byte(get_bumper(1));
push_byte(get_bumper(2));
send_checksum();
// Now 3 + 3 + 3 + 3 + 1 = 13
}
