/*
* \brief Function gives the control of RF states to MAC.
*
* \param new_state RF state
* \param rf_channel RF channel
*
* \return 0 Success
*/
static int8_t rf_interface_state_control(phy_interface_state_e new_state, uint8_t rf_channel)
{
int8_t ret_val = 0;
switch (new_state)
{
/*Reset PHY driver and set to idle*/
case PHY_INTERFACE_RESET:
break;
/*Disable PHY Interface driver*/
case PHY_INTERFACE_DOWN:
rf_shutdown();
break;
/*Enable PHY Interface driver*/
case PHY_INTERFACE_UP:
rf_channel_set(rf_channel);
rf_receive();
break;
/*Enable wireless interface ED scan mode*/
case PHY_INTERFACE_RX_ENERGY_STATE:
break;
case PHY_INTERFACE_SNIFFER_STATE: /**< Enable Sniffer state */
rf_mode = RF_MODE_SNIFFER;
rf_channel_set(rf_channel);
rf_flags_clear(RFF_RX);
rf_receive();
break;
}
return ret_val;
}
void calibrate(void)
{
mrf.SetChannel(15);
int i;
while (!status)
{
sprintf(txBuffer, "Switch\r\n");
rf_send(txBuffer, strlen(txBuffer) + 1);
printf("Sent:%s\r\n", txBuffer);
wait_ms(30);
for (i=0; i<1; i++)
{
rxLen = rf_receive(rxBuffer, 128);
printf("Received Length:%d\r\n", rxLen);
while (rxLen <= 0)
{
printf("Listening\r\n");
rxLen = rf_receive(rxBuffer, 128);
}
active = rxBuffer[8] - 48;
}
if (active == 1)
{
printf("Switch Done!\r\n");
status = 1;
//break;
}
}
//mrf.SetChannel(com_channel);
}
/****************************************
* main
*
* sets up the communication between the main mbed
* continuously checks for a command from the main mbed
* if new command received, then executes the appropriate function
*
****************************************/
int main()
{
pc.baud(9600);
clear_strip();
//tlc.setLED(0, 0, 65535, 65535);
// tlc.setLED(1, 0, 65535, 65535);
// tlc.write();
// wait(3);
// clear_strip();
create_pattern(P_FADE, 65535, 0, 65535, 25, 2000);
while(1)
{
rxLen = rf_receive(rxBuffer, 128);
if(rxLen > 0)
{
if(rxBuffer[0] == ID)
{
char* rxBuffer1 = &rxBuffer[1];
pc.printf("Received string %s \r\n",rxBuffer);
parser(rxBuffer1);
}
}
}
}
static int8_t rf_interface_state_control(phy_interface_state_e new_state, uint8_t rf_channel)
{
platform_enter_critical();
switch (new_state)
{
/*Reset PHY driver and set to idle*/
case PHY_INTERFACE_RESET:
rf_abort();
break;
/*Disable PHY Interface driver*/
case PHY_INTERFACE_DOWN:
rf_abort();
break;
/*Enable PHY Interface driver*/
case PHY_INTERFACE_UP:
if (PhyPlmeSetCurrentChannelRequest(rf_channel, 0)) {
return 1;
}
rf_receive();
break;
/*Enable wireless interface ED scan mode*/
case PHY_INTERFACE_RX_ENERGY_STATE:
if (PhyPlmeSetCurrentChannelRequest(rf_channel, 0)) {
return 1;
}
rf_abort();
rf_mac_ed_state_enable();
break;
case PHY_INTERFACE_SNIFFER_STATE: /**< Enable Sniffer state */
rf_promiscuous(1);
if (PhyPlmeSetCurrentChannelRequest(rf_channel, 0)) {
return 1;
}
rf_receive();
break;
}
platform_exit_critical();
return 0;
}
/*
* \brief Function is a call back for ACK wait timeout.
*
* \param none
*
* \return none
*/
void rf_ack_wait_timer_interrupt(void)
{
platform_enter_critical();
/*Force PLL state*/
rf_if_change_trx_state(FORCE_PLL_ON);
rf_poll_trx_state_change(PLL_ON);
/*Start receiver in RX_AACK_ON state*/
rf_rx_mode = 0;
rf_flags_clear(RFF_RX);
rf_receive();
platform_exit_critical();
}
/*
motor1 : LT
motor2 : LB
motor3 : RT
motor4 : RB
motor5 : BACK
motor6 : BACK
*/
int main() {
OSCclass *c=new OSCclass;
OSCmsg *recv;
uint8_t channel = 7;
// Set the Channel. 0 is the default, 15 is max
//mrf.SetChannel(channel);
char add[5];
char *command;
pc.printf("Start----- Haptic Vest!\r\n");
while(1) {
rxLen = rf_receive(rxBuffer, 128);
// pc.printf("RxLen %d", rxLen);
if(rxLen > 0) {
recv= c->getOSCmsg(rxBuffer);
printf("Address is %s with type %c and msg %c \r\n",recv->getAddr(),recv->getType(),recv->getArgs());
strncpy(add,recv->getAddr(),5);
if(add[1] == 'B'){
motorStatus = find_status(add);
pc.printf("motor status: %d\r\n", motorStatus);
motor6 = motorStatus%10;//BACK
//motor5 = motor6;
motor4 = (motorStatus / 10) % 10;//RB
motor3 = (motorStatus / 100) % 10;//RT
//solenoid2=motor3;
motor2 = (motorStatus / 1000) % 10;//LB
motor1 = (motorStatus / 10000) % 10;//LT
//solenoid1=motor1;
//printf("%d %d %d %d %d %d \r\n",motor1,motor2,motor3,motor4,motor5,motor6);
updateMotors();
}
}
}
}
int main (void) {
uint8_t channel = 2;
//The wheels do not spin at same speed so calibration is required
calibrate_right = 4/3;
//Set the Channel. 0 is default, 15 is max
mrf.SetChannel(channel);
//Start the timer
timer.start();
while(true) {
//Try to receive some data
rxLen = rf_receive(rxBuffer, 128);
if(rxLen > 0) {
pc.printf("Received: %s\r\n", rxBuffer);
}
//Send some data every .1 second
if(timer.read_ms() >= 100) {
//Reset the timer to 0
timer.reset();
//Calibration to get the full range of joystick
if(LeftV.read() <= 0.35 || LeftV.read() >= 0.65) {
L = (0.5 - LeftV.read()) * 300;
S = 300;
}
//If close to middle (.5+error) then do nothing
else {
L = 0;
S = 0;
}
//More calibration
if((1.0 - RightV.read()) <= 0.35 || (1.0 - RightV.read()) >= 0.65) {
R = (0.5 - (1.0 - RightV.read())) * 300 * calibrate_right;
S = 300;
}
else {
R = 0;
}
//Add the setmotor command to the buffer
sprintf(txBuffer, "setmotor %d %d %d", L, R, S);
//Send the buffer
rf_send(txBuffer, strlen(txBuffer) + 1);
pc.printf("Sent: %s\r\n", txBuffer);
}
}
}
/*
* \brief Function is a call back for cca interval timer.
*
* \param none
*
* \return none
*/
void rf_cca_timer_interrupt(void)
{
/*Start CCA process*/
if(rf_if_read_trx_state() == BUSY_RX_AACK)
{
arm_net_phy_tx_done(rf_radio_driver_id, mac_tx_handle, PHY_LINK_CCA_FAIL, 1, 1);
}
else
{
/*Set radio in RX state to read channel*/
rf_receive();
rf_if_enable_cca_ed_done_interrupt();
rf_if_start_cca_process();
}
}
/*
* \brief Function is a call back for TX end interrupt.
*
* \param none
*
* \return none
*/
void rf_handle_tx_end(void)
{
phy_link_tx_status_e phy_status = PHY_LINK_TX_SUCCESS;
rf_rx_mode = 0;
/*If ACK is needed for this transmission*/
if(need_ack && rf_flags_check(RFF_TX))
{
rf_ack_wait_timer_start(rf_ack_wait_duration);
rf_rx_mode = 1;
}
rf_flags_clear(RFF_RX);
/*Start receiver*/
rf_receive();
/*Call PHY TX Done API*/
arm_net_phy_tx_done(rf_radio_driver_id, mac_tx_handle, phy_status, 1, 1);
}
/*
* \brief Function initialises the radio driver and resets the radio.
*
* \param none
*
* \return none
*/
void rf_init(void)
{
/*Reset RF module*/
rf_if_reset_radio();
/*Write RF settings*/
rf_write_settings();
/*Initialise PHY mode*/
rf_init_phy_mode();
/*Clear RF flags*/
rf_flags_reset();
/*Set RF in TRX OFF state*/
rf_if_change_trx_state(TRX_OFF);
/*Set RF in PLL_ON state*/
rf_if_change_trx_state(PLL_ON);
/*Start receiver*/
rf_receive();
/*Read random variable. This will be used when seeding pseudo-random generator*/
rf_rnd_rssi = rf_if_read_rnd();
/*Start RF calibration timer*/
rf_calibration_timer_start(RF_CALIBRATION_INTERVAL);
}
/*
* \brief Function calibrates the radio.
*
* \param none
*
* \return none
*/
void rf_calibration_cb(void)
{
/*clear tuned flag to start tuning in rf_receive*/
rf_tuned = 0;
/*If RF is in default receive state, start calibration*/
if(rf_if_read_trx_state() == RX_AACK_ON)
{
platform_enter_critical();
/*Set RF in PLL_ON state*/
rf_if_change_trx_state(PLL_ON);
/*Set RF in TRX_OFF state to start PLL tuning*/
rf_if_change_trx_state(TRX_OFF);
/*Set RF in RX_ON state to calibrate*/
rf_if_change_trx_state(RX_ON);
/*Calibrate FTN*/
rf_if_calibration();
/*RF is tuned now*/
rf_tuned = 1;
/*Back to default receive state*/
rf_flags_clear(RFF_RX);
rf_receive();
platform_exit_critical();
}
}
/*
* \brief Function is a call back for RX end interrupt.
*
* \param none
*
* \return none
*/
void rf_handle_rx_end(void)
{
uint8_t rf_lqi = 0;
int8_t rf_rssi = 0;
/*Start receiver*/
rf_flags_clear(RFF_RX);
rf_receive();
/*Frame received interrupt*/
if(rf_flags_check(RFF_RX))
{
/*Check CRC_valid bit*/
if(rf_if_check_crc())
{
uint8_t *rf_rx_ptr;
uint8_t receiving_ack = 0;
/*Read length*/
uint8_t len = rf_if_read_received_frame_length();
rf_rx_ptr = rf_buffer;
/*ACK frame*/
if(len == 5)
{
/*Read ACK in static ACK buffer*/
receiving_ack = 1;
}
/*Check the length is valid*/
if(len > 1 && len < RF_BUFFER_SIZE)
{
/*Read received packet*/
rf_if_read_packet(rf_rx_ptr, len);
if(!receiving_ack)
{
rf_rssi = rf_if_read_rssi();
/*Scale LQI using received RSSI*/
rf_lqi = rf_scale_lqi(rf_rssi);
}
if(rf_mode == RF_MODE_SNIFFER)
{
arm_net_phy_rx(PHY_LAYER_PAYLOAD,rf_buffer,len - 2, rf_lqi, rf_rssi, rf_radio_driver_id);
}
else
{
/*Handle received ACK*/
if(receiving_ack && ((rf_buffer[0] & 0x07) == 0x02))
{
uint8_t pending = 0;
/*Check if data is pending*/
if ((rf_buffer[0] & 0x10))
{
pending=1;
}
/*Send sequence number in ACK handler*/
rf_handle_ack(rf_buffer[2], pending);
}
else
{
arm_net_phy_rx(PHY_LAYER_PAYLOAD,rf_buffer,len - 2, rf_lqi, rf_rssi, rf_radio_driver_id);
}
}
}
}
}
}
/****************************************
* main
*
* sets up the communication between the main mbed
* inits the leds to check if the leds are working properly
* continuously checks for a command from the main mbed
* if new command received, then executes the appropriate function
*
****************************************/
int main()
{
uint8_t R,G,B;
pc.baud(9600);
strip.begin();
init_grid();
/*Shutting down the leds*/
clear_strip();
getColor(&R,&G,&B,"G");
wait_ms(10);
//clear_strip();
// show_pixel(&pixel_grid[0][0], R, G, B, 500);
// wait_ms(1000);
pixel_grid[0][0].selected = 1;
pixel_grid[1][1].selected = 1;
pixel_grid[2][2].selected = 1;
pixel_grid[2][0].selected = 1;
pixel_grid[0][2].selected = 1;
//pixel_grid[0][0].selected = 1;
// pixel_grid[1][1].selected = 1;
// pixel_grid[2][2].selected = 1;
// pixel_grid[3][3].selected = 1;
// pixel_grid[4][2].selected = 1;
// pixel_grid[5][1].selected = 1;
// pixel_grid[6][0].selected = 1;
// select_all_pixels();
create_pattern(P_WAVE_FLOW, DIR_RIGHT, R, G, B, 50, 5000);
// pixel_grid[10][0].selected = 1;
// pixel_grid[10][1].selected = 1;
// pixel_grid[10][2].selected = 1;
// pixel_grid[10][3].selected = 1;
// pixel_grid[9][1].selected = 1;
// pixel_grid[9][2].selected = 1;
// pixel_grid[9][3].selected = 1;
// pixel_grid[8][2].selected = 1;
// pixel_grid[8][3].selected = 1;
// pixel_grid[7][3].selected = 1;
//
// create_pattern(P_CUSTOM, DIR_UP, 150, 80, 200, 50, 1000);
//
//pixel_grid[0][0].selected = 1;
// pixel_grid[1][1].selected = 1;
// pixel_grid[2][2].selected = 1;
// pixel_grid[2][0].selected = 1;
// pixel_grid[0][2].selected = 1;
//
// getColor(&R,&G,&B,"G");
//
// create_pattern(P_CUSTOM, DIR_UP, R, G, B, 25, 1000);
//
// pixel_grid[2][0].selected = 1;
// pixel_grid[2][1].selected = 1;
// pixel_grid[2][2].selected = 1;
// pixel_grid[2][3].selected = 1;
//
// getColor(&R,&G,&B,"B");
//
// create_pattern(P_CUSTOM, DIR_UP, R, G, B, 50, 1000);
// wait(0.1);
clear_strip();
while(1)
{
//wait for messages
rxLen = rf_receive(rxBuffer, 128);
if(rxLen > 0)
{
if(rxBuffer[0] == ID)
{
char* rxBuffer1 = &rxBuffer[1];
pc.printf("Received string %s \r\n",rxBuffer);
parser(rxBuffer1);
}
}
}
}
void main(void)
{
//***************** BEGINNING OF ETHERNET SETUP [DONT EDIT] *****************//
EthernetInterface eth;
eth.init(); //Use DHCP
eth.connect();
printf("IP Address is %s\n", eth.getIPAddress());
UDPSocket server;
server.bind(ECHO_SERVER_PORT);
Endpoint client;
char buffer[256];
//***************** END OF ETHERNET SETUP **********************************//
//***************** BEGINNING OF WIRELESS SETUP [DONT EDIT] *****************//
uint8_t channel = 6;
//Set the Channel. 0 is default, 15 is max
mrf.SetChannel(channel);
//Start the timer
timer.start();
//***************** END OF WIRELESS SETUP **********************************//
raiseAllPins();
dropAllPins();
speedChecker.attach(&speedLogic,0.005); //sets ticker for interrupts
dcOUT=0; // turns DC motor off initially
int cycles = 0;
int flush = 0;
int startChar =0;
while(1) {
dcPWM.write(dutyCycle);
cycles = cycles-1;
if(needsInput==true) {
//What MBED is receiving from client?
if(flush) {
int lengthBuffer = strlen(buffer);
for(int i = 0; i <lengthBuffer; i++) {
buffer[i]='\0';
}
}
printf("\nWait for packet...\n\r");
lightShow.drawChar('@'); //this should draw the character on the screen
int n = server.receiveFrom(client, buffer, sizeof(buffer));
printf("\nReceived packet...\n\r");
printf("\nReceived integer n...%i\n\r",n);
buffer[n]='\0';
printf("\nyour word is: %s\n\r",buffer);
server.sendTo(client, buffer, n); //echo protocol
needsInput=false;
cycles=n;
/*revive code*/
sendDelay = slowSpeed; //pins
dutyCycle = slowMotor; //motor
dcOUT = 1; //turn on motor
off=false;
slow = true;
startChar=0;
}
if(cycles<=0) {
needsInput= true;
slow = false;
off=true;
dcOUT = 0; //turn motor off
sendDelay = slowSpeed; //pins
dutyCycle = slowMotor; //for whenever it turns on after you turned it off using REDUCE
}
if(!off) {
char character = getNextCharacter(startChar, buffer);
startChar=1;
printf("\nchar: %c\n\r",character);
int pinBinary = braille.matchCharacter(character);
if(braille.isNumber(character)) {
handleNumberCase(character);
}
led1=1;
lightShow.drawChar(character); //this should draw the character on the screen
led1=0;
wait_ms(sendDelay);
sendNumber(pinBinary);
printf("Pinbinary: %i\n\r",pinBinary);
//***** ACKNOWLEDGE CODE ******//
int recLength = rf_receive(rxbuffer,128);
while(recLength<=0) {
led2=1;
recLength = rf_receive(rxbuffer,128);
}
led2=0;
//***** END ACKNOWLEDGE CODE ******//
}//end if motor stopped code
dropAllPins();
}//end while loop
}//end all code
int main (void)
{
pc.baud(115200);
RoboState1 = Charging;
RoboState2 = Charging;
State = Picking;
int xpos = x_min;
int ypos = x_max;
float xre = 0;
float yre = 0;
while(true){
switch(State){
default:
State = Picking;
break;
case 1: //Done
State = Done;
break;
case 2: //Picking
mrf.SetChannel(channel1);
rxLen = rf_receive(rxBuffer, 128);
State = Picking;
if (rxLen>0 && rxBuffer[0] == 'R'){ //if robot1 is ready
wait(0.5);
RoboState1 = Working;
sprintf(txBuffer, "%d %d %d %d %d\r\n",xpos,ypos,y_max,y_min,x_max);
rf_send(txBuffer, strlen(txBuffer) + 1);
State = Waiting;
}else{
mrf.SetChannel(channel2);
rxLen = rf_receive(rxBuffer, 128);
if (rxLen>0 && rxBuffer[0] == 'R'){ //else if robot2 is ready
wait(0.5);
sprintf(txBuffer, "%d %d %d %d %d\r\n",xpos,ypos,y_max,y_min,x_max);
RoboState2 = Working;
rf_send(txBuffer, strlen(txBuffer) + 1);
State = Waiting;
}
}
break;
case 3: //Waiting
rxLen = rf_receive(rxBuffer, 128);
State = Waiting;
if(rxLen > 0) {
led1 = led1^1;
pc.printf("%s", rxBuffer);
if(rxBuffer[0] == 'D'){
sscanf(rxBuffer, "Died: %f,%f\r\n", &xre, &yre);
xpos = int(xre);
ypos = int(yre);
State = Waiting;
}
if(rxBuffer[0] == 'F'){
State = Done;
}
if(rxBuffer[0] == 'C'){
if (RoboState1 == Working){
RoboState1 = Charging;
}
if (RoboState2 == Working){
RoboState2 == Charging;
}
State = Picking;
}
}
break;
}
}
}
//***************** You can start coding here *****************//
int main (void)
{
// MRF SPI Init
uint8_t channel = 12;
print_adxl_timer.start();
mrf.SetChannel(channel); //Set the Channel. 0 is default, 15 is max
// ///////////
// M2 SPI Init
ss = 1;
spi.format(8, 0);
//spi.format(16, 0);
spi.frequency(SPI_FREQ);
spi_start_ticker.attach_us(&SPI_start_handler, SPI_PRD*1000);
read_adxl = false;
// ///////////
// IMU I2C Init
i2c.frequency(400000); // 400KHz
setup_imu();
// ///////////
//Start the timer
timer.start();
debounce.start();
strcpy(testString, "0,2,0.2,1,0.6,0.7,0.7,0.4,0");
err();
testcheck = 1;
button.rise(debounce_handler);
training_mode = false;
// Initial Lights
recMode = 0;
// Timer initiate
for (int i = 0; i < 7; i++) {
//timerarray[i].start();
timerarray[i].stop();
}
// Digital Pin Pwms
for (int i = 0; i < 7; i++) {
pwm_duty_cycles[i] = 0.0;
}
ticker_30Hz.attach_us(&ticker_30Hz_handler, THIRTYHZ*1000);
pc.printf("Starting...\n");
while(1) {
greenlightact();
if (training_mode) {
if (read_adxl) read_adxl_from_m2();
if (print_adxl_timer.read_ms() > PRINT_PRD) {
print_adxl_timer.reset();
pc.printf("dx: ");
for (int i = 0; i < NUM_ADXL; i++)
pc.printf("%d\t", adxl[i]);
pc.printf("\tdtheta: ");
for (int i = 0; i < NUM_IMU; i++)
pc.printf("%d\t", imu[i]);
pc.printf("\t");
for (int i = 0; i < 7; i++)
pc.printf("%d\t", vhit[i]);
pc.printf("\n");
}
// GUI controlled
//TODO TODO TODO
if (pc.readable()) {
pc.gets(gui_input_buf, 16);
sscanf(gui_input_buf, "%d,%d,%d,%d,%d,%d,%d,%d", &recMode,green,green+1,green+2,green+3,green+4,green+5,green+6);
pc.printf("DEBUG: %d,%d,%d,%d,%d,%d,%d,%d\n", recMode,green[0],green[1],green[2],green[3],green[4],green[5],green[6]);
//pc.printf("DEBUG: %d,%d,%d,%d,%d,%d,%d,%d\r\n", *recMode,green[0],green[1],green[2],green[3],green[4],green[5],green[6]);
if (recMode > 0) {
trigger(recMode);
}
}
pc.printf("DEBUG: %d,%d,%d,%d,%d,%d,%d,%d\n", recMode,green[0],green[1],green[2],green[3],green[4],green[5],green[6]);
}
else {
//Try to receive some data
rxLen = rf_receive(rxBuffer, 128);
if(rxLen > 0) {
pc.printf("%s \r\n", rxBuffer );
token = strtok(rxBuffer, " ,");
player = atoi(token);
if (player == LINKED_PLAYER) {
token = strtok(NULL, " ,");
inputs = atoi(token);
for (int i = 0; i < inputs; i++) {
token = strtok(NULL, ",");
float temp = atof(token);
//float temp = ((float) atoi(token))/MAX_ACCEL_VAL;
temp = 1.0 - temp;
pwm_duty_cycles[i] = temp;
if (temp > 0.2) {
green[i] = 0;
}
else {
green[i] = 1;
}
}
}
}
}
}
}
int main(){
Buzzer.write(0);
mrf.SetChannel(com_channel);
clock_init();
cam_init();
pc.baud(115200);
State = WAITING;
Battery_Level = Battery_Time;
t_battery.reset();
int start_x, start_y, y_min, y_max, dest_x;
active = 1;
mrf.SetChannel(15);
for (int i=0; i<3; i++){ //load the tile it is under;
rxLen = rf_receive(rxBuffer, 128);
while (rxLen <= 0){
rxLen = rf_receive(rxBuffer, 128);
}
sscanf(rxBuffer,"active: %d\r\n", &active);
}
pc.printf("the active value is: %d\r\n", active);
mrf.SetChannel(com_channel);
while(1){
switch(State){ //use a state machine to control the behavior or a slave
default:
State = WAITING;
break;
case 1: //Waiting
sprintf(txBuffer, "Ready\r\n");
rf_send(txBuffer, strlen(txBuffer) + 1);
robot.stop();
rxLen = rf_receive(rxBuffer, 128);
if (rxLen > 0){
int res = sscanf(rxBuffer, "%d %d %d %d %d\r\n", &start_x, &start_y, &y_min, &y_max, &dest_x);
if (res){
sprintf(txBuffer, "Yes\r\n");
rf_send(txBuffer, strlen(txBuffer) + 1);
State = WORKING;
}
}else{
State = WAITING;
}
break;
case 2: //Working
Buzzer.write(1);
wait(0.2);
Buzzer.write(0);
go_to(start_x, start_y);
Buzzer.write(1);
wait(0.6);
Buzzer.write(0);
if (zig_zag(y_max, y_min, dest_x) > 0){
robot.stop();
sprintf(txBuffer, "Finished\r\n");
rf_send(txBuffer, strlen(txBuffer) + 1);
State = WAITING;
}else{
robot.stop();
rotate(90);
Locate(position);
sprintf(txBuffer, "Died: %3.2f,%3.2f\r\n",position[0],position[1]);
rf_send(txBuffer, strlen(txBuffer) + 1);
for (int b=0; b<3; b++){
Buzzer.write(1);
wait(0.2);
Buzzer.write(0);
wait(0.2);
}
go_to(Charging_X, Charging_Y);
State = CHARGING;
}
break;
case 3: //charging
sprintf(txBuffer, "Charging: %d\r\n",Battery_Level);
rf_send(txBuffer, strlen(txBuffer) + 1);
rxLen = rf_receive(rxBuffer, 128);
if (rxLen > 0){
int res = sscanf(rxBuffer, "%d %d %d %d %d\r\n", &start_x, &start_y, &y_min, &y_max, &dest_x);
if (res){
State = WORKING;
}
}else{
State = CHARGING;
}
wait(1);
Battery_Level += 3;
if (Battery_Level >= Battery_Time){
Battery_Level = Battery_Time;
State = WAITING;
}
break;
}
}
}
