car chassis
Dependencies: Servo mbed-rtos mbed
body.cpp@2:7dfc8dd6aab3, 2015-10-08 (annotated)
- Committer:
- mariob
- Date:
- Thu Oct 08 13:36:17 2015 +0000
- Revision:
- 2:7dfc8dd6aab3
- Parent:
- 1:79b1ee0f97ef
- Child:
- 3:bfc20ec72b15
added body implementation
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mariob | 0:ce6055872f4e | 1 | #include "car_config.hpp" |
mariob | 2:7dfc8dd6aab3 | 2 | #include "net.hpp" |
mariob | 2:7dfc8dd6aab3 | 3 | #include "can.hpp" |
mariob | 0:ce6055872f4e | 4 | #include "mbed.h" |
mariob | 0:ce6055872f4e | 5 | #include "rtos.h" |
mariob | 0:ce6055872f4e | 6 | |
mariob | 2:7dfc8dd6aab3 | 7 | /*** LOCAL DEFINES ***/ |
mariob | 0:ce6055872f4e | 8 | |
mariob | 2:7dfc8dd6aab3 | 9 | //configuration of the GP2D12 device |
mariob | 0:ce6055872f4e | 10 | #define k_5 12466.0 |
mariob | 0:ce6055872f4e | 11 | #define k_4 -23216.0 |
mariob | 0:ce6055872f4e | 12 | #define k_3 14974.0 |
mariob | 0:ce6055872f4e | 13 | #define k_2 -3585.0 |
mariob | 0:ce6055872f4e | 14 | #define k_1 19.0 |
mariob | 0:ce6055872f4e | 15 | #define k_0 96.0 |
mariob | 0:ce6055872f4e | 16 | |
mariob | 2:7dfc8dd6aab3 | 17 | /*** LOCAL DATA ***/ |
mariob | 2:7dfc8dd6aab3 | 18 | |
mariob | 2:7dfc8dd6aab3 | 19 | static AnalogIn back_r(HW_REAR_RIGHT_EYE); |
mariob | 2:7dfc8dd6aab3 | 20 | static AnalogIn back_l(HW_REAR_LEFT_EYE); |
mariob | 2:7dfc8dd6aab3 | 21 | static I2C i2c(HW_FRONT_EYE_RX, HW_FRONT_EYE_TX); |
mariob | 2:7dfc8dd6aab3 | 22 | static DigitalIn hit_front(HW_HIT_FRONT); |
mariob | 2:7dfc8dd6aab3 | 23 | static DigitalIn hit_rear(HW_HIT_REAR); |
mariob | 2:7dfc8dd6aab3 | 24 | static DigitalIn hit_left(HW_HIT_LEFT); |
mariob | 2:7dfc8dd6aab3 | 25 | static DigitalIn hit_right(HW_HIT_RIGHT); |
mariob | 2:7dfc8dd6aab3 | 26 | |
mariob | 2:7dfc8dd6aab3 | 27 | /*** LOCAL FUNCTION PROTOTYPES ***/ |
mariob | 0:ce6055872f4e | 28 | |
mariob | 2:7dfc8dd6aab3 | 29 | //read from the analog input GP2D12 |
mariob | 2:7dfc8dd6aab3 | 30 | uint8 read_back_distance (AnalogIn* side); |
mariob | 2:7dfc8dd6aab3 | 31 | |
mariob | 2:7dfc8dd6aab3 | 32 | //initialize the SRF10 |
mariob | 2:7dfc8dd6aab3 | 33 | void srf10_start (char addr); |
mariob | 2:7dfc8dd6aab3 | 34 | |
mariob | 2:7dfc8dd6aab3 | 35 | //change the address of the SRF10 |
mariob | 2:7dfc8dd6aab3 | 36 | void srf10_change_address (char actual_addr, char new_addr); |
mariob | 2:7dfc8dd6aab3 | 37 | |
mariob | 2:7dfc8dd6aab3 | 38 | //read from the SRF10 |
mariob | 2:7dfc8dd6aab3 | 39 | uint16 srf10_read (char addr); |
mariob | 2:7dfc8dd6aab3 | 40 | |
mariob | 2:7dfc8dd6aab3 | 41 | //handle of error condition: switch all hardware off |
mariob | 2:7dfc8dd6aab3 | 42 | void stop_body() { |
mariob | 0:ce6055872f4e | 43 | } |
mariob | 0:ce6055872f4e | 44 | |
mariob | 2:7dfc8dd6aab3 | 45 | /*** GLOBAL FUNTIONS ***/ |
mariob | 0:ce6055872f4e | 46 | |
mariob | 2:7dfc8dd6aab3 | 47 | void init_body() { |
mariob | 2:7dfc8dd6aab3 | 48 | srf10_start(HW_FRONT_EYE_DEFAULT_ADDR); |
mariob | 2:7dfc8dd6aab3 | 49 | srf10_change_address(HW_FRONT_EYE_DEFAULT_ADDR, HW_FRONT_EYE_ADDR); |
mariob | 0:ce6055872f4e | 50 | } |
mariob | 0:ce6055872f4e | 51 | |
mariob | 2:7dfc8dd6aab3 | 52 | void thread_body (void const *args) { |
mariob | 2:7dfc8dd6aab3 | 53 | while(1) { |
mariob | 2:7dfc8dd6aab3 | 54 | if (can_msg_is_missing(CAN_MISSING_CMD_BODY_ID)) |
mariob | 2:7dfc8dd6aab3 | 55 | stop_body(); |
mariob | 2:7dfc8dd6aab3 | 56 | else { |
mariob | 2:7dfc8dd6aab3 | 57 | //if a message has been received, set lights |
mariob | 2:7dfc8dd6aab3 | 58 | if (can_cmd_body.flag == CAN_FLAG_RECEIVED) { |
mariob | 2:7dfc8dd6aab3 | 59 | int r = can_cmd_body.payload.msg.light_r; |
mariob | 2:7dfc8dd6aab3 | 60 | int l = can_cmd_body.payload.msg.light_l; |
mariob | 2:7dfc8dd6aab3 | 61 | int c = can_cmd_body.payload.msg.light_c; |
mariob | 2:7dfc8dd6aab3 | 62 | can_cmd_body.flag = CAN_FLAG_EMPTY; |
mariob | 2:7dfc8dd6aab3 | 63 | } |
mariob | 2:7dfc8dd6aab3 | 64 | } |
mariob | 2:7dfc8dd6aab3 | 65 | //send sensors' values |
mariob | 2:7dfc8dd6aab3 | 66 | can_sts_body.payload.msg.eye_back_l = read_back_distance(&back_l); |
mariob | 2:7dfc8dd6aab3 | 67 | can_sts_body.payload.msg.eye_back_r = read_back_distance(&back_r); |
mariob | 2:7dfc8dd6aab3 | 68 | can_sts_body.payload.msg.eye_front = srf10_read(HW_FRONT_EYE_ADDR); |
mariob | 2:7dfc8dd6aab3 | 69 | can_sts_body.payload.msg.hit_front = hit_front.read(); |
mariob | 2:7dfc8dd6aab3 | 70 | can_sts_body.payload.msg.hit_rear = hit_rear.read(); |
mariob | 2:7dfc8dd6aab3 | 71 | can_sts_body.payload.msg.hit_left = hit_left.read(); |
mariob | 2:7dfc8dd6aab3 | 72 | can_sts_body.payload.msg.hit_right = hit_right.read(); |
mariob | 2:7dfc8dd6aab3 | 73 | /* |
mariob | 2:7dfc8dd6aab3 | 74 | printf("HIT: %d %d %d %d\n\r", hit_front.read(), |
mariob | 2:7dfc8dd6aab3 | 75 | hit_rear.read(), |
mariob | 2:7dfc8dd6aab3 | 76 | hit_left.read(), |
mariob | 2:7dfc8dd6aab3 | 77 | hit_right.read()); |
mariob | 2:7dfc8dd6aab3 | 78 | printf("EYE: %d %d %d\n\r", can_sts_body.payload.msg.eye_back_l, |
mariob | 2:7dfc8dd6aab3 | 79 | can_sts_body.payload.msg.eye_back_r, |
mariob | 2:7dfc8dd6aab3 | 80 | can_sts_body.payload.msg.eye_front); |
mariob | 2:7dfc8dd6aab3 | 81 | */ |
mariob | 2:7dfc8dd6aab3 | 82 | Thread::wait(BODY_THREAD_PERIOD); |
mariob | 2:7dfc8dd6aab3 | 83 | } |
mariob | 0:ce6055872f4e | 84 | } |
mariob | 0:ce6055872f4e | 85 | |
mariob | 2:7dfc8dd6aab3 | 86 | /*** LOCAL FUNCTIONS ***/ |
mariob | 0:ce6055872f4e | 87 | |
mariob | 2:7dfc8dd6aab3 | 88 | uint8 read_back_distance (AnalogIn* side) { |
mariob | 2:7dfc8dd6aab3 | 89 | float x = side->read(); |
mariob | 2:7dfc8dd6aab3 | 90 | float res = 0.0; |
mariob | 2:7dfc8dd6aab3 | 91 | res += k_5 * (x * x * x * x * x); |
mariob | 2:7dfc8dd6aab3 | 92 | res += k_4 * (x * x * x * x); |
mariob | 2:7dfc8dd6aab3 | 93 | res += k_3 * (x * x * x); |
mariob | 2:7dfc8dd6aab3 | 94 | res += k_2 * (x * x); |
mariob | 2:7dfc8dd6aab3 | 95 | res += k_1 * x; |
mariob | 2:7dfc8dd6aab3 | 96 | res += k_0; |
mariob | 2:7dfc8dd6aab3 | 97 | if (res < 0.0) |
mariob | 2:7dfc8dd6aab3 | 98 | return 0; |
mariob | 2:7dfc8dd6aab3 | 99 | if (res > 255.0) |
mariob | 2:7dfc8dd6aab3 | 100 | return res; |
mariob | 2:7dfc8dd6aab3 | 101 | return (uint8)res; |
mariob | 2:7dfc8dd6aab3 | 102 | } |
mariob | 0:ce6055872f4e | 103 | |
mariob | 2:7dfc8dd6aab3 | 104 | void srf10_start (char addr) { |
mariob | 2:7dfc8dd6aab3 | 105 | char cmd[2]; |
mariob | 2:7dfc8dd6aab3 | 106 | |
mariob | 2:7dfc8dd6aab3 | 107 | //set range: register x 43mm + 43mm |
mariob | 2:7dfc8dd6aab3 | 108 | cmd[0] = 0x02; |
mariob | 2:7dfc8dd6aab3 | 109 | cmd[1] = 0x30; //register = 48*43mm+43mm=2107mm |
mariob | 2:7dfc8dd6aab3 | 110 | i2c.write(addr, cmd, 2); |
mariob | 2:7dfc8dd6aab3 | 111 | |
mariob | 2:7dfc8dd6aab3 | 112 | cmd[0] = 0x01; //receiver gain register |
mariob | 2:7dfc8dd6aab3 | 113 | cmd[1] = 0x10; //maximum gain |
mariob | 2:7dfc8dd6aab3 | 114 | i2c.write(addr, cmd, 2); |
mariob | 0:ce6055872f4e | 115 | } |
mariob | 0:ce6055872f4e | 116 | |
mariob | 2:7dfc8dd6aab3 | 117 | void srf10_change_address (char addr, char new_addr) { |
mariob | 2:7dfc8dd6aab3 | 118 | char cmd[2]; |
mariob | 0:ce6055872f4e | 119 | |
mariob | 2:7dfc8dd6aab3 | 120 | cmd[0] = 0x00; |
mariob | 2:7dfc8dd6aab3 | 121 | cmd[1] = 0xA0; |
mariob | 2:7dfc8dd6aab3 | 122 | i2c.write(addr, cmd, 2); |
mariob | 2:7dfc8dd6aab3 | 123 | cmd[0] = 0x00; |
mariob | 2:7dfc8dd6aab3 | 124 | cmd[1] = 0xAA; |
mariob | 2:7dfc8dd6aab3 | 125 | i2c.write(addr, cmd, 2); |
mariob | 2:7dfc8dd6aab3 | 126 | cmd[0] = 0x00; |
mariob | 2:7dfc8dd6aab3 | 127 | cmd[1] = 0xA5; |
mariob | 2:7dfc8dd6aab3 | 128 | i2c.write(addr, cmd, 2); |
mariob | 2:7dfc8dd6aab3 | 129 | cmd[0] = 0x00; |
mariob | 2:7dfc8dd6aab3 | 130 | cmd[1] = new_addr; |
mariob | 2:7dfc8dd6aab3 | 131 | i2c.write(addr, cmd, 2); |
mariob | 2:7dfc8dd6aab3 | 132 | } |
mariob | 0:ce6055872f4e | 133 | |
mariob | 2:7dfc8dd6aab3 | 134 | uint16 srf10_read (char addr) { |
mariob | 2:7dfc8dd6aab3 | 135 | char cmd[2]; |
mariob | 2:7dfc8dd6aab3 | 136 | char echo[2]; |
mariob | 2:7dfc8dd6aab3 | 137 | |
mariob | 2:7dfc8dd6aab3 | 138 | cmd[0] = 0x00; //Command register |
mariob | 2:7dfc8dd6aab3 | 139 | cmd[1] = 0x51; //Ranging results in cm |
mariob | 2:7dfc8dd6aab3 | 140 | i2c.write(addr, cmd, 2); // Send ranging burst |
mariob | 0:ce6055872f4e | 141 | |
mariob | 2:7dfc8dd6aab3 | 142 | wait(0.07); // Wait for return echo |
mariob | 0:ce6055872f4e | 143 | |
mariob | 2:7dfc8dd6aab3 | 144 | cmd[0] = 0x02; //Address of first echo |
mariob | 2:7dfc8dd6aab3 | 145 | i2c.write(addr, cmd, 1, 1); //Send address of first echo |
mariob | 2:7dfc8dd6aab3 | 146 | i2c.read(addr, echo, 2); //Read two-byte echo result |
mariob | 2:7dfc8dd6aab3 | 147 | |
mariob | 2:7dfc8dd6aab3 | 148 | return (echo[0]<<8)+echo[1]; |
mariob | 0:ce6055872f4e | 149 | } |