Diff: body.cpp

Revision:

0:ce6055872f4e

Child:

1:79b1ee0f97ef

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/body.cpp	Sun Aug 02 12:55:33 2015 +0000
@@ -0,0 +1,131 @@
+#include "car_config.hpp"
+#include "mbed.h"
+#include "rtos.h"
+
+extern can_cmd_body_t can_cmd_body;
+extern can_sts_body_t can_sts_body;
+
+AnalogIn back_r (p19);
+AnalogIn back_l (p20);
+I2C i2c(p28, p27);
+char addr;
+
+uint8 read_back_distance (AnalogIn* side);
+void srf10_start ();
+void srf10_change_address ();
+uint16 srf10_read ();
+
+
+#define k_5 12466.0
+#define k_4 -23216.0
+#define k_3 14974.0
+#define k_2 -3585.0
+#define k_1 19.0
+#define k_0 96.0
+
+void init_body()
+{
+    srf10_start();
+    srf10_change_address();
+}
+
+void stop_body()
+{
+    printf("MISSING BODY!!!\r\n");
+}
+
+void thread_body (void const *args)
+{
+    while(1) {
+        printf("BODY\r\n");
+        if (can_msg_is_missing(CAN_MISSING_BODY_ID))
+            stop_body();
+        else {
+            if (can_cmd_body.flag == CAN_FLAG_RECEIVED) {
+                int r = can_cmd_body.payload.msg.light_r;
+                int l = can_cmd_body.payload.msg.light_l;
+                int c = can_cmd_body.payload.msg.light_c;
+                printf("READ CMD_BODY: %d %d %d\r\n", r, l, c);
+                can_cmd_body.flag = CAN_FLAG_EMPTY;    
+            }
+            static int x = 0;
+            can_sts_body.payload.msg.eye_back_l = read_back_distance(&back_l);
+            can_sts_body.payload.msg.eye_back_r = read_back_distance(&back_r);
+            can_sts_body.payload.msg.eye_front = srf10_read();
+            can_sts_body.payload.buf[0] = x;
+            x++;
+        }
+        Thread::wait(1000);
+    }
+}
+
+uint8 read_back_distance (AnalogIn* side)
+{
+    float x = side->read();
+    float res = 0.0;
+    res += k_5 * (x * x * x * x * x);
+    res += k_4 * (x * x * x * x);
+    res += k_3 * (x * x * x);
+    res += k_2 * (x * x);
+    res += k_1 *  x;
+    res += k_0;
+    if (res<0)
+        return 0;
+    if (res>255)
+        return res;
+    return (uint8)res;
+}
+
+void srf10_start ()
+{
+    char cmd[2];
+
+    addr = 0xE0;
+
+    //set range: register x 43mm + 43mm
+    cmd[0] = 0x02;
+    cmd[1] = 0x30; //register = 48*43mm+43mm=2107mm
+    i2c.write(addr, cmd, 2);
+
+    cmd[0] = 0x01; //Receiver gain register
+    cmd[1] = 0x10; //maximum gain
+    i2c.write(addr, cmd, 2);
+}
+
+void srf10_change_address ()
+{
+    char cmd[2];
+
+    cmd[0] = 0x00;
+    cmd[1] = 0xA0;
+    i2c.write(addr, cmd, 2);
+    cmd[0] = 0x00;
+    cmd[1] = 0xAA;
+    i2c.write(addr, cmd, 2);
+    cmd[0] = 0x00;
+    cmd[1] = 0xA5;
+    i2c.write(addr, cmd, 2);
+    cmd[0] = 0x00;
+    cmd[1] = 0xF2;
+    i2c.write(addr, cmd, 2);
+
+    addr = 0xF2;
+}
+
+uint16 srf10_read ()
+{
+    char cmd[2];
+    char echo[2];
+
+    cmd[0] = 0x00;                      // Command register
+    cmd[1] = 0x51;                      // Ranging results in cm
+    i2c.write(addr, cmd, 2);          // Send ranging burst
+
+    wait(0.07);                         // Wait for return echo
+
+    cmd[0] = 0x02;                        // Address of first echo
+    i2c.write(addr, cmd, 1, 1);         // Send address of first echo
+    i2c.read(addr, echo, 2);            // Read two-byte echo result
+
+    return (echo[0]<<8)+echo[1];
+}