Mario Bambagini / Mbed 2 deprecated car_chassis

car chassis

Dependencies:   Servo mbed-rtos mbed

body.cpp@1:79b1ee0f97ef, 2015-08-31 (annotated)

Committer:
mariob
Date:
Mon Aug 31 22:25:57 2015 +0000
Revision:
1:79b1ee0f97ef
Parent:
0:ce6055872f4e
Child:
2:7dfc8dd6aab3
first commit with the following features: CAN driver (it works), eeprom (it works but it is not used)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mariob 0:ce6055872f4e 1 #include "car_config.hpp"
mariob 0:ce6055872f4e 2 #include "mbed.h"
mariob 0:ce6055872f4e 3 #include "rtos.h"
mariob 0:ce6055872f4e 4
mariob 0:ce6055872f4e 5 extern can_cmd_body_t can_cmd_body;
mariob 0:ce6055872f4e 6 extern can_sts_body_t can_sts_body;
mariob 0:ce6055872f4e 7
mariob 0:ce6055872f4e 8 AnalogIn back_r (p19);
mariob 0:ce6055872f4e 9 AnalogIn back_l (p20);
mariob 0:ce6055872f4e 10 I2C i2c(p28, p27);
mariob 0:ce6055872f4e 11 char addr;
mariob 0:ce6055872f4e 12
mariob 0:ce6055872f4e 13 uint8 read_back_distance (AnalogIn* side);
mariob 0:ce6055872f4e 14 void srf10_start ();
mariob 0:ce6055872f4e 15 void srf10_change_address ();
mariob 0:ce6055872f4e 16 uint16 srf10_read ();
mariob 0:ce6055872f4e 17
mariob 0:ce6055872f4e 18
mariob 0:ce6055872f4e 19 #define k_5 12466.0
mariob 0:ce6055872f4e 20 #define k_4 -23216.0
mariob 0:ce6055872f4e 21 #define k_3 14974.0
mariob 0:ce6055872f4e 22 #define k_2 -3585.0
mariob 0:ce6055872f4e 23 #define k_1 19.0
mariob 0:ce6055872f4e 24 #define k_0 96.0
mariob 0:ce6055872f4e 25
mariob 0:ce6055872f4e 26 void init_body()
mariob 0:ce6055872f4e 27 {
mariob 0:ce6055872f4e 28 srf10_start();
mariob 0:ce6055872f4e 29 srf10_change_address();
mariob 0:ce6055872f4e 30 }
mariob 0:ce6055872f4e 31
mariob 0:ce6055872f4e 32 void stop_body()
mariob 0:ce6055872f4e 33 {
mariob 0:ce6055872f4e 34 printf("MISSING BODY!!!\r\n");
mariob 0:ce6055872f4e 35 }
mariob 0:ce6055872f4e 36
mariob 0:ce6055872f4e 37 void thread_body (void const *args)
mariob 0:ce6055872f4e 38 {
mariob 0:ce6055872f4e 39 while(1) {
mariob 0:ce6055872f4e 40 printf("BODY\r\n");
mariob 0:ce6055872f4e 41 if (can_msg_is_missing(CAN_MISSING_BODY_ID))
mariob 0:ce6055872f4e 42 stop_body();
mariob 0:ce6055872f4e 43 else {
mariob 0:ce6055872f4e 44 if (can_cmd_body.flag == CAN_FLAG_RECEIVED) {
mariob 0:ce6055872f4e 45 int r = can_cmd_body.payload.msg.light_r;
mariob 0:ce6055872f4e 46 int l = can_cmd_body.payload.msg.light_l;
mariob 0:ce6055872f4e 47 int c = can_cmd_body.payload.msg.light_c;
mariob 0:ce6055872f4e 48 printf("READ CMD_BODY: %d %d %d\r\n", r, l, c);
mariob 1:79b1ee0f97ef 49 can_cmd_body.flag = CAN_FLAG_EMPTY;
mariob 0:ce6055872f4e 50 }
mariob 0:ce6055872f4e 51 static int x = 0;
mariob 0:ce6055872f4e 52 can_sts_body.payload.msg.eye_back_l = read_back_distance(&back_l);
mariob 0:ce6055872f4e 53 can_sts_body.payload.msg.eye_back_r = read_back_distance(&back_r);
mariob 0:ce6055872f4e 54 can_sts_body.payload.msg.eye_front = srf10_read();
mariob 0:ce6055872f4e 55 can_sts_body.payload.buf[0] = x;
mariob 0:ce6055872f4e 56 x++;
mariob 0:ce6055872f4e 57 }
mariob 0:ce6055872f4e 58 Thread::wait(1000);
mariob 0:ce6055872f4e 59 }
mariob 0:ce6055872f4e 60 }
mariob 0:ce6055872f4e 61
mariob 0:ce6055872f4e 62 uint8 read_back_distance (AnalogIn* side)
mariob 0:ce6055872f4e 63 {
mariob 0:ce6055872f4e 64 float x = side->read();
mariob 0:ce6055872f4e 65 float res = 0.0;
mariob 0:ce6055872f4e 66 res += k_5 * (x * x * x * x * x);
mariob 0:ce6055872f4e 67 res += k_4 * (x * x * x * x);
mariob 0:ce6055872f4e 68 res += k_3 * (x * x * x);
mariob 0:ce6055872f4e 69 res += k_2 * (x * x);
mariob 0:ce6055872f4e 70 res += k_1 * x;
mariob 0:ce6055872f4e 71 res += k_0;
mariob 0:ce6055872f4e 72 if (res<0)
mariob 0:ce6055872f4e 73 return 0;
mariob 0:ce6055872f4e 74 if (res>255)
mariob 0:ce6055872f4e 75 return res;
mariob 0:ce6055872f4e 76 return (uint8)res;
mariob 0:ce6055872f4e 77 }
mariob 0:ce6055872f4e 78
mariob 0:ce6055872f4e 79 void srf10_start ()
mariob 0:ce6055872f4e 80 {
mariob 0:ce6055872f4e 81 char cmd[2];
mariob 0:ce6055872f4e 82
mariob 0:ce6055872f4e 83 addr = 0xE0;
mariob 0:ce6055872f4e 84
mariob 0:ce6055872f4e 85 //set range: register x 43mm + 43mm
mariob 0:ce6055872f4e 86 cmd[0] = 0x02;
mariob 0:ce6055872f4e 87 cmd[1] = 0x30; //register = 48*43mm+43mm=2107mm
mariob 0:ce6055872f4e 88 i2c.write(addr, cmd, 2);
mariob 0:ce6055872f4e 89
mariob 0:ce6055872f4e 90 cmd[0] = 0x01; //Receiver gain register
mariob 0:ce6055872f4e 91 cmd[1] = 0x10; //maximum gain
mariob 0:ce6055872f4e 92 i2c.write(addr, cmd, 2);
mariob 0:ce6055872f4e 93 }
mariob 0:ce6055872f4e 94
mariob 0:ce6055872f4e 95 void srf10_change_address ()
mariob 0:ce6055872f4e 96 {
mariob 0:ce6055872f4e 97 char cmd[2];
mariob 0:ce6055872f4e 98
mariob 0:ce6055872f4e 99 cmd[0] = 0x00;
mariob 0:ce6055872f4e 100 cmd[1] = 0xA0;
mariob 0:ce6055872f4e 101 i2c.write(addr, cmd, 2);
mariob 0:ce6055872f4e 102 cmd[0] = 0x00;
mariob 0:ce6055872f4e 103 cmd[1] = 0xAA;
mariob 0:ce6055872f4e 104 i2c.write(addr, cmd, 2);
mariob 0:ce6055872f4e 105 cmd[0] = 0x00;
mariob 0:ce6055872f4e 106 cmd[1] = 0xA5;
mariob 0:ce6055872f4e 107 i2c.write(addr, cmd, 2);
mariob 0:ce6055872f4e 108 cmd[0] = 0x00;
mariob 0:ce6055872f4e 109 cmd[1] = 0xF2;
mariob 0:ce6055872f4e 110 i2c.write(addr, cmd, 2);
mariob 0:ce6055872f4e 111
mariob 0:ce6055872f4e 112 addr = 0xF2;
mariob 0:ce6055872f4e 113 }
mariob 0:ce6055872f4e 114
mariob 0:ce6055872f4e 115 uint16 srf10_read ()
mariob 0:ce6055872f4e 116 {
mariob 0:ce6055872f4e 117 char cmd[2];
mariob 0:ce6055872f4e 118 char echo[2];
mariob 0:ce6055872f4e 119
mariob 0:ce6055872f4e 120 cmd[0] = 0x00; // Command register
mariob 0:ce6055872f4e 121 cmd[1] = 0x51; // Ranging results in cm
mariob 0:ce6055872f4e 122 i2c.write(addr, cmd, 2); // Send ranging burst
mariob 0:ce6055872f4e 123
mariob 0:ce6055872f4e 124 wait(0.07); // Wait for return echo
mariob 0:ce6055872f4e 125
mariob 0:ce6055872f4e 126 cmd[0] = 0x02; // Address of first echo
mariob 0:ce6055872f4e 127 i2c.write(addr, cmd, 1, 1); // Send address of first echo
mariob 0:ce6055872f4e 128 i2c.read(addr, echo, 2); // Read two-byte echo result
mariob 0:ce6055872f4e 129
mariob 0:ce6055872f4e 130 return (echo[0]<<8)+echo[1];
mariob 0:ce6055872f4e 131 }