// {{{ // // Copyright (C) 2013 Nicolas Schodet // // APBTeam: // Web: http://apbteam.org/ // Email: team AT apbteam DOT org // // This program is free software; you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation; either version 2 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. // // }}} #include "lcd.hh" #include "ucoolib/arch/arch.hh" #include "ucoolib/utils/delay.hh" #include #include #include #include "ucoolib/hal/i2c/i2c.hh" #include "ucoolib/utils/crc.hh" #include "ucoolib/utils/bytes.hh" //uint16_t ucoo::bytes_pack (arg 1 ,arg 2 ); static char i2c_color[3]; static int i2c_time; static char i2c_cmd[100]="..."; static Rect pos_r;//position du robot static int nb_obs=0;//nb d'obstacle static Rect pos_obs[4]; static Rect pos_r_n; //next position of the robot static const int i2c_status_size = 3; static const int i2c_command_size = 16; static uint8_t i2c_seq; static bool i2c_received; /// Handle a command received by I2C. bool conv_pos (Rect &pos) { pos.x=(3000-pos.x) *320/3000; pos.y=(2000-pos.y) *240/2000; if(LCD::belong(pos.x,pos.y)) return false; return true; } void i2c_handle (LCD &lcd, const char *buf, int size) { // Command too small. if (size < 3) return; // Check CRC. if (ucoo::crc8_compute ((const uint8_t *) buf + 1, size - 1) != buf[0]) return; // Handle sequence number. if (buf[1] != 0 && buf[1] == i2c_seq) // Duplicated command. return; // OK, now handle command. char cmd = buf[2]; const char *arg = &buf[3]; int arg_nb = size - 3; int j;//for incrementation switch (cmd) { case 'c': // Team color. if (arg_nb != 3) return; i2c_color[0]=arg[0]; i2c_color[1]=arg[1]; i2c_color[2]=arg[2]; break; case 't': if(arg[0]>90) strcpy(i2c_cmd,"ERROR I2C time"); i2c_time=arg[0]; break; case 'p': //position of the robot pos_r.x=ucoo::bytes_pack (arg[0] ,arg [1] );//position in mm pos_r.y=ucoo::bytes_pack (arg[2] ,arg [3] ); if(!conv_pos(pos_r)) strcpy(i2c_cmd,"ERROR I2C position"); break; case 'm': //message strcpy(i2c_cmd,arg); break; case 'o': //obstacle (barrier) j=0; nb_obs=arg[0]; for(int i=1 ; i<=nb_obs*4 ; i=i+4) { pos_obs[j].x=ucoo::bytes_pack (arg[i] ,arg [i+1] ); pos_obs[j].y=ucoo::bytes_pack (arg[i+2] ,arg [i+3] ); j++; if(!conv_pos(pos_obs[j])) strcpy(i2c_cmd,"ERROR I2C obstacle"); } break; case 'n'://next position of the robot pos_r_n.x=ucoo::bytes_pack (arg[0] ,arg [1] ); pos_r_n.y=ucoo::bytes_pack (arg[2] ,arg [3] ); if(!conv_pos(pos_r_n)) strcpy(i2c_cmd,"ERROR I2C next pos"); break; default: // Unknown command. return; } i2c_received = true; // Acknowledge. if (buf[1] != 0) i2c_seq = buf[1]; } /// Poll I2C interface for commands and update status. void i2c_poll (LCD &lcd, ucoo::I2cSlaveDataBuffer &i2c_data) { char buf[i2c_command_size]; int size; // Handle incoming commands. while ((size = i2c_data.poll (buf, sizeof (buf)))) i2c_handle (lcd, buf, size); // Update status. char status[i2c_status_size]; status[1] = i2c_seq; status[2] = 0; status[0] = ucoo::crc8_compute ((const uint8_t *) &status[1], sizeof (status) - 1); i2c_data.update (status, sizeof (status)); } void draw_bar (LCD lcd)//draw barrier (make a red circle where is approximatly the barrier) { for(int i=0; i i2c_data; i2c.register_data (0x20, i2c_data); //Init global variable i2c_color[0]=0;i2c_color[1]=0;i2c_color[2]=0; i2c_time=90; pos_r_n.x=160;pos_r_n.y=120; pos_r.x=160;pos_r.y=120; // Init. LCD lcd; //ucoo::delay_ms (1000); draw_table (lcd); // Wait orders. while (1) { if (i2c_received) { draw_table (lcd);//draw the table draw_robot (lcd);//draw the robot and his destination on the table i2c_received = false; } i2c_poll (lcd, i2c_data); ucoo::delay_ms (4); } }