Mario Bambagini / Mbed 2 deprecated car_chassis

car chassis

Dependencies:   Servo mbed-rtos mbed

body.cpp@4:7fa7f78cbb92, 2015-10-13 (annotated)

Committer:
mariob
Date:
Tue Oct 13 14:08:44 2015 +0000
Revision:
4:7fa7f78cbb92
Parent:
3:bfc20ec72b15 
delete unused library

Who changed what in which revision?

UserRevisionLine numberNew 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 3:bfc20ec72b15 59 //int r = can_cmd_body.payload.msg.light_r;
mariob 3:bfc20ec72b15 60 //int l = can_cmd_body.payload.msg.light_l;
mariob 3:bfc20ec72b15 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 }