car chassis
Dependencies: Servo mbed-rtos mbed
body.cpp@0:ce6055872f4e, 2015-08-02 (annotated)
- Committer:
- mariob
- Date:
- Sun Aug 02 12:55:33 2015 +0000
- Revision:
- 0:ce6055872f4e
- Child:
- 1:79b1ee0f97ef
first commit: new car ECU
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:ce6055872f4e | 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 | } |