body.cpp

00001 #include "car_config.hpp"
00002 #include "net.hpp"
00003 #include "can.hpp"
00004 #include "mbed.h"
00005 #include "rtos.h"
00006 
00007 /*** LOCAL DEFINES ***/
00008 
00009 //configuration of the GP2D12 device
00010 #define k_5 12466.0
00011 #define k_4 -23216.0
00012 #define k_3 14974.0
00013 #define k_2 -3585.0
00014 #define k_1 19.0
00015 #define k_0 96.0
00016 
00017 /*** LOCAL DATA ***/
00018 
00019 static AnalogIn back_r(HW_REAR_RIGHT_EYE);
00020 static AnalogIn back_l(HW_REAR_LEFT_EYE);
00021 static I2C i2c(HW_FRONT_EYE_RX, HW_FRONT_EYE_TX);
00022 static DigitalIn hit_front(HW_HIT_FRONT);
00023 static DigitalIn hit_rear(HW_HIT_REAR);
00024 static DigitalIn hit_left(HW_HIT_LEFT);
00025 static DigitalIn hit_right(HW_HIT_RIGHT);
00026 
00027 /*** LOCAL FUNCTION PROTOTYPES ***/
00028 
00029 //read from the analog input GP2D12
00030 uint8 read_back_distance (AnalogIn* side);
00031 
00032 //initialize the SRF10
00033 void srf10_start (char addr);
00034 
00035 //change the address of the SRF10
00036 void srf10_change_address (char actual_addr, char new_addr);
00037 
00038 //read from the SRF10
00039 uint16 srf10_read (char addr);
00040 
00041 //handle of error condition: switch all hardware off
00042 void stop_body() {
00043 }
00044 
00045 /*** GLOBAL FUNTIONS ***/
00046 
00047 void init_body() {
00048   srf10_start(HW_FRONT_EYE_DEFAULT_ADDR);
00049   srf10_change_address(HW_FRONT_EYE_DEFAULT_ADDR, HW_FRONT_EYE_ADDR);
00050 }
00051 
00052 void thread_body (void const *args) {
00053   while(1) {
00054     if (can_msg_is_missing(CAN_MISSING_CMD_BODY_ID))
00055       stop_body();
00056     else {
00057       //if a message has been received, set lights
00058       if (can_cmd_body.flag == CAN_FLAG_RECEIVED) {
00059         //int r = can_cmd_body.payload.msg.light_r;
00060         //int l = can_cmd_body.payload.msg.light_l;
00061         //int c = can_cmd_body.payload.msg.light_c;
00062         can_cmd_body.flag = CAN_FLAG_EMPTY;
00063       }
00064     }
00065     //send sensors' values
00066     can_sts_body.payload.msg.eye_back_l = read_back_distance(&back_l);
00067     can_sts_body.payload.msg.eye_back_r = read_back_distance(&back_r);
00068     can_sts_body.payload.msg.eye_front = srf10_read(HW_FRONT_EYE_ADDR);
00069     can_sts_body.payload.msg.hit_front = hit_front.read();
00070     can_sts_body.payload.msg.hit_rear = hit_rear.read();
00071     can_sts_body.payload.msg.hit_left = hit_left.read();
00072     can_sts_body.payload.msg.hit_right = hit_right.read();
00073 /*
00074     printf("HIT: %d %d %d %d\n\r", hit_front.read(),
00075                                    hit_rear.read(),
00076                                    hit_left.read(),
00077                                    hit_right.read());
00078     printf("EYE: %d %d %d\n\r", can_sts_body.payload.msg.eye_back_l,
00079                                 can_sts_body.payload.msg.eye_back_r, 
00080                                 can_sts_body.payload.msg.eye_front);
00081 */
00082     Thread::wait(BODY_THREAD_PERIOD);
00083   }
00084 }
00085 
00086 /*** LOCAL FUNCTIONS ***/
00087 
00088 uint8 read_back_distance (AnalogIn* side) {
00089   float x = side->read();
00090   float res = 0.0;
00091   res += k_5 * (x * x * x * x * x);
00092   res += k_4 * (x * x * x * x);
00093   res += k_3 * (x * x * x);
00094   res += k_2 * (x * x);
00095   res += k_1 *  x;
00096   res += k_0;
00097   if (res < 0.0)
00098     return 0;
00099   if (res > 255.0)
00100     return res;
00101   return (uint8)res;
00102 }
00103 
00104 void srf10_start (char addr) {
00105   char cmd[2];
00106 
00107   //set range: register x 43mm + 43mm
00108   cmd[0] = 0x02;
00109   cmd[1] = 0x30; //register = 48*43mm+43mm=2107mm
00110   i2c.write(addr, cmd, 2);
00111 
00112   cmd[0] = 0x01; //receiver gain register
00113   cmd[1] = 0x10; //maximum gain
00114   i2c.write(addr, cmd, 2);
00115 }
00116 
00117 void srf10_change_address (char addr, char new_addr) {
00118   char cmd[2];
00119 
00120   cmd[0] = 0x00;
00121   cmd[1] = 0xA0;
00122   i2c.write(addr, cmd, 2);
00123   cmd[0] = 0x00;
00124   cmd[1] = 0xAA;
00125   i2c.write(addr, cmd, 2);
00126   cmd[0] = 0x00;
00127   cmd[1] = 0xA5;
00128   i2c.write(addr, cmd, 2);
00129   cmd[0] = 0x00;
00130   cmd[1] = new_addr;
00131   i2c.write(addr, cmd, 2);
00132 }
00133 
00134 uint16 srf10_read (char addr) {
00135   char cmd[2];
00136   char echo[2];
00137 
00138   cmd[0] = 0x00; //Command register
00139   cmd[1] = 0x51; //Ranging results in cm
00140   i2c.write(addr, cmd, 2); // Send ranging burst
00141 
00142   wait(0.07); // Wait for return echo
00143 
00144   cmd[0] = 0x02; //Address of first echo
00145   i2c.write(addr, cmd, 1, 1); //Send address of first echo
00146   i2c.read(addr, echo, 2); //Read two-byte echo result
00147 
00148   return (echo[0]<<8)+echo[1];
00149 }