José Gouveia / Mbed OS SRA2020-2021

feito

Dependencies:   BufferedSerial

Robot.cpp@5:31605c9c05f8, 2021-05-14 (annotated)

Committer:
josefg25
Date:
Fri May 14 16:50:34 2021 +0000
Revision:
5:31605c9c05f8
Parent:
4:0fac5e5d08f7
Child:
6:af79dc3c6fc2
feito;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yaaqobhpt 0:c25c4b67b6a1 1 #include "Robot.h"
yaaqobhpt 0:c25c4b67b6a1 2 #include "mbed.h"
yaaqobhpt 0:c25c4b67b6a1 3
josefg25 5:31605c9c05f8 4 I2C i2c(I2C_SDA, I2C_SCL);
yaaqobhpt 0:c25c4b67b6a1 5 const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
yaaqobhpt 0:c25c4b67b6a1 6
yaaqobhpt 0:c25c4b67b6a1 7 int16_t countsLeft = 0;
yaaqobhpt 0:c25c4b67b6a1 8 int16_t countsRight = 0;
yaaqobhpt 0:c25c4b67b6a1 9
josefg25 4:0fac5e5d08f7 10 struct Robot{
josefg25 3:892592b2c2ea 11 //Robot properties
josefg25 3:892592b2c2ea 12 float worldTrobot[3][3];
josefg25 3:892592b2c2ea 13 //Wheels properties
josefg25 3:892592b2c2ea 14 float nWheels;
josefg25 3:892592b2c2ea 15 float robotTwheels;
josefg25 3:892592b2c2ea 16 float wheels2Twheels;
josefg25 3:892592b2c2ea 17 float wheelsPoints;
josefg25 3:892592b2c2ea 18 float hWheels;
josefg25 3:892592b2c2ea 19 //Platform properties
josefg25 3:892592b2c2ea 20 float platformRadius;
josefg25 3:892592b2c2ea 21 float hPlatform;
josefg25 4:0fac5e5d08f7 22 };
josefg25 3:892592b2c2ea 23
josefg25 5:31605c9c05f8 24 void getPose(struct Robot Robot,float *pose[3][1]){
josefg25 5:31605c9c05f8 25 pose[0][0] = Robot.worldTrobot[0][2];
josefg25 5:31605c9c05f8 26 pose[1][0] = Robot.worldTrobot[1][2];
josefg25 5:31605c9c05f8 27 pose[2][0] = atan2(Robot.worldTrobot[1][0],Robot.worldTrobot[0][0]);
josefg25 3:892592b2c2ea 28 }
josefg25 3:892592b2c2ea 29
josefg25 3:892592b2c2ea 30 void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float *w){
josefg25 3:892592b2c2ea 31 w[0]=(vRobot-(wheelsDistance/2)*wRobot)/wheelsRadius;
josefg25 3:892592b2c2ea 32 w[1]=(vRobot+(wheelsDistance/2)*wRobot)/wheelsRadius;
josefg25 3:892592b2c2ea 33 }
josefg25 3:892592b2c2ea 34
josefg25 4:0fac5e5d08f7 35 void nextPose(float T,float *w,float wRobot,float *poseRobot,float wheelsRadius,float wheelsDistance,float *nextPose){
josefg25 3:892592b2c2ea 36 nextPose[0] = poseRobot[0]+(T*wheelsRadius*((w[1]+w[0]/2))*cos(poseRobot[2]+(wRobot*T/2)));
josefg25 3:892592b2c2ea 37 nextPose[1] = poseRobot[1]+(T*wheelsRadius*((w[1]+w[0]/2))*sin(poseRobot[2]+(wRobot*T/2)));
josefg25 3:892592b2c2ea 38 nextPose[2] = poseRobot[2]+(T*wheelsRadius*(w[1]-w[0])/wheelsDistance);
josefg25 3:892592b2c2ea 39 }
josefg25 3:892592b2c2ea 40
josefg25 5:31605c9c05f8 41 void robot_init(struct Robot *r, float rPos_a[],float rTheta,float nW){
josefg25 4:0fac5e5d08f7 42 r->worldTrobot[0][0] = cos(rTheta);
josefg25 4:0fac5e5d08f7 43 r->worldTrobot[0][1] = -sin(rTheta);
josefg25 5:31605c9c05f8 44 r->worldTrobot[0][2] = rPos_a[0];
josefg25 4:0fac5e5d08f7 45 r->worldTrobot[1][0] = sin(rTheta);
josefg25 4:0fac5e5d08f7 46 r->worldTrobot[1][1] = cos(rTheta);
josefg25 5:31605c9c05f8 47 r->worldTrobot[1][2] = rPos_a[1];
josefg25 4:0fac5e5d08f7 48 r->worldTrobot[2][0] = 0;
josefg25 4:0fac5e5d08f7 49 r->worldTrobot[2][1] = 0;
josefg25 4:0fac5e5d08f7 50 r->worldTrobot[2][2] = 1;
josefg25 3:892592b2c2ea 51
josefg25 4:0fac5e5d08f7 52 r->nWheels=nW;
josefg25 3:892592b2c2ea 53 }
josefg25 3:892592b2c2ea 54
josefg25 4:0fac5e5d08f7 55 int inverse_range_sensor_model(float d,float theta,float atual_x,float atual_y,int cell_x,int cell_y){
josefg25 4:0fac5e5d08f7 56 //Variaveis
josefg25 4:0fac5e5d08f7 57 //d têm de ser em cm
josefg25 4:0fac5e5d08f7 58 float z_max=200; //2m
josefg25 4:0fac5e5d08f7 59 float beta=2*(3.14159265358979323846/180.0);
josefg25 4:0fac5e5d08f7 60 float alpha=5; //5cm
josefg25 4:0fac5e5d08f7 61 float l_occ=0.65;
josefg25 4:0fac5e5d08f7 62 float l_free=-0.65;
josefg25 4:0fac5e5d08f7 63 float phi=0;
josefg25 4:0fac5e5d08f7 64 float r = 0;
josefg25 4:0fac5e5d08f7 65 float aux_min=0;
josefg25 4:0fac5e5d08f7 66
josefg25 4:0fac5e5d08f7 67 r = sqrt((double)((pow(cell_x*5-atual_x,2) + pow(cell_y*5-atual_y,2))));
josefg25 4:0fac5e5d08f7 68 phi=atan2(cell_y*5-atual_y,cell_x*5-atual_x)-theta;
josefg25 4:0fac5e5d08f7 69
josefg25 4:0fac5e5d08f7 70 if(z_max<(d+alpha/2)){
josefg25 4:0fac5e5d08f7 71 aux_min=z_max;
josefg25 4:0fac5e5d08f7 72 }
josefg25 4:0fac5e5d08f7 73 else{
josefg25 4:0fac5e5d08f7 74 aux_min=d+alpha/2;
josefg25 4:0fac5e5d08f7 75 }
josefg25 4:0fac5e5d08f7 76
josefg25 4:0fac5e5d08f7 77 float odds=0;
josefg25 4:0fac5e5d08f7 78
josefg25 4:0fac5e5d08f7 79 if (d<z_max && r>d){
josefg25 4:0fac5e5d08f7 80 odds=l_occ;
josefg25 4:0fac5e5d08f7 81 }
josefg25 4:0fac5e5d08f7 82 else if (r<=d){
josefg25 4:0fac5e5d08f7 83 odds=l_free;
josefg25 4:0fac5e5d08f7 84 }
josefg25 4:0fac5e5d08f7 85 else if ((r>aux_min) || (abs(phi - theta)>beta/2)){
josefg25 4:0fac5e5d08f7 86 odds=0;
josefg25 4:0fac5e5d08f7 87 }
josefg25 4:0fac5e5d08f7 88 return odds;
josefg25 4:0fac5e5d08f7 89 }
josefg25 4:0fac5e5d08f7 90
josefg25 5:31605c9c05f8 91 void occupancy_grid_mapping(float *log_mapa[40][40], float d,float theta,float actual_x,float actual_y,float angle,int i,int j){
josefg25 4:0fac5e5d08f7 92 angle = (angle+90) * (3.14159265358979323846 / 180.0);
josefg25 4:0fac5e5d08f7 93 float angle_total = angle + theta;
josefg25 3:892592b2c2ea 94
josefg25 3:892592b2c2ea 95 log_mapa[i][j] = log_mapa[i][j] + inverse_range_sensor_model(d,angle_total,actual_x,actual_y,i,j);
josefg25 3:892592b2c2ea 96 }
josefg25 3:892592b2c2ea 97
josefg25 3:892592b2c2ea 98 void ComputeLidarPoint(float *pose, int *PointDetected, float distance, float angle){
josefg25 3:892592b2c2ea 99 float angle_tot, angle_tot_deg;
josefg25 3:892592b2c2ea 100 //Angle Retification
josefg25 3:892592b2c2ea 101 angle = angle-90;
josefg25 3:892592b2c2ea 102 if (angle < 0){
josefg25 3:892592b2c2ea 103 angle = 360+angle;
josefg25 3:892592b2c2ea 104 }
josefg25 3:892592b2c2ea 105
josefg25 4:0fac5e5d08f7 106 angle_tot = angle * (3.14159265358979323846/180.0);
josefg25 3:892592b2c2ea 107
josefg25 3:892592b2c2ea 108 angle_tot = angle_tot+pose[2];
josefg25 4:0fac5e5d08f7 109 angle_tot_deg = angle_tot * (180.0/3.14159265358979323846);
josefg25 3:892592b2c2ea 110
josefg25 4:0fac5e5d08f7 111 if ((angle_tot_deg >= 0) && (angle_tot_deg < 89.99)){
josefg25 3:892592b2c2ea 112 PointDetected[0] = ceil(pose[0]-cos(angle_tot))/5;
josefg25 4:0fac5e5d08f7 113 PointDetected[1] = ceil(pose[1]+sin(angle_tot))/5;
josefg25 3:892592b2c2ea 114 }
josefg25 4:0fac5e5d08f7 115 else if((angle_tot_deg >= 90) && (angle_tot_deg < 179.99)){
josefg25 3:892592b2c2ea 116 PointDetected[0] = ceil(pose[0]-cos(angle_tot))/5;
josefg25 4:0fac5e5d08f7 117 PointDetected[1] = ceil(pose[1]-sin(angle_tot))/5;
josefg25 3:892592b2c2ea 118 }
josefg25 4:0fac5e5d08f7 119 else if((angle_tot_deg >= 180) && (angle_tot_deg < 269.99)){
josefg25 3:892592b2c2ea 120 PointDetected[0] = ceil(pose[0]+cos(angle_tot))/5;
josefg25 4:0fac5e5d08f7 121 PointDetected[1] = ceil(pose[1]-sin(angle_tot))/5;
josefg25 3:892592b2c2ea 122 }
josefg25 4:0fac5e5d08f7 123 else if((angle_tot_deg >= 270) && (angle_tot_deg < 359.99)){
josefg25 3:892592b2c2ea 124 PointDetected[0] = ceil(pose[0]+cos(angle_tot))/5;
josefg25 4:0fac5e5d08f7 125 PointDetected[1] = ceil(pose[1]+sin(angle_tot))/5;
josefg25 3:892592b2c2ea 126 }
josefg25 3:892592b2c2ea 127 }
josefg25 3:892592b2c2ea 128
josefg25 4:0fac5e5d08f7 129 void setPose(Robot *r, float *pose){
josefg25 4:0fac5e5d08f7 130 r->worldTrobot[0][0] = cos(pose[3]);
josefg25 4:0fac5e5d08f7 131 r->worldTrobot[0][1] = -sin(pose[3]);
josefg25 4:0fac5e5d08f7 132 r->worldTrobot[0][2] = pose[1];
josefg25 3:892592b2c2ea 133
josefg25 4:0fac5e5d08f7 134 r->worldTrobot[1][0] = sin(pose[3]);
josefg25 4:0fac5e5d08f7 135 r->worldTrobot[1][1] = cos(pose[3]);
josefg25 4:0fac5e5d08f7 136 r->worldTrobot[1][2] = pose[2];
josefg25 3:892592b2c2ea 137
josefg25 4:0fac5e5d08f7 138 r->worldTrobot[2][0] = 0;
josefg25 4:0fac5e5d08f7 139 r->worldTrobot[2][1] = 0;
josefg25 4:0fac5e5d08f7 140 r->worldTrobot[2][2] = 1;
josefg25 3:892592b2c2ea 141 }
josefg25 3:892592b2c2ea 142
yaaqobhpt 0:c25c4b67b6a1 143 void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
yaaqobhpt 0:c25c4b67b6a1 144 {
yaaqobhpt 0:c25c4b67b6a1 145 char buffer[5];
yaaqobhpt 0:c25c4b67b6a1 146
yaaqobhpt 0:c25c4b67b6a1 147 buffer[0] = 0xA1;
yaaqobhpt 0:c25c4b67b6a1 148 memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed));
yaaqobhpt 0:c25c4b67b6a1 149 memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed));
yaaqobhpt 0:c25c4b67b6a1 150
yaaqobhpt 0:c25c4b67b6a1 151 i2c.write(addr8bit, buffer, 5); // 5 bytes
yaaqobhpt 0:c25c4b67b6a1 152 }
yaaqobhpt 0:c25c4b67b6a1 153
yaaqobhpt 0:c25c4b67b6a1 154 void setLeftSpeed(int16_t speed)
yaaqobhpt 0:c25c4b67b6a1 155 {
yaaqobhpt 0:c25c4b67b6a1 156 char buffer[3];
yaaqobhpt 0:c25c4b67b6a1 157
yaaqobhpt 0:c25c4b67b6a1 158 buffer[0] = 0xA2;
yaaqobhpt 0:c25c4b67b6a1 159 memcpy(&buffer[1], &speed, sizeof(speed));
yaaqobhpt 0:c25c4b67b6a1 160
yaaqobhpt 0:c25c4b67b6a1 161 i2c.write(addr8bit, buffer, 3); // 3 bytes
yaaqobhpt 0:c25c4b67b6a1 162 }
yaaqobhpt 0:c25c4b67b6a1 163
yaaqobhpt 0:c25c4b67b6a1 164 void setRightSpeed(int16_t speed)
yaaqobhpt 0:c25c4b67b6a1 165 {
yaaqobhpt 0:c25c4b67b6a1 166 char buffer[3];
yaaqobhpt 0:c25c4b67b6a1 167
yaaqobhpt 0:c25c4b67b6a1 168 buffer[0] = 0xA3;
yaaqobhpt 0:c25c4b67b6a1 169 memcpy(&buffer[1], &speed, sizeof(speed));
yaaqobhpt 0:c25c4b67b6a1 170
yaaqobhpt 0:c25c4b67b6a1 171 i2c.write(addr8bit, buffer, 3); // 3 bytes
yaaqobhpt 0:c25c4b67b6a1 172 }
yaaqobhpt 0:c25c4b67b6a1 173
yaaqobhpt 0:c25c4b67b6a1 174 void getCounts()
yaaqobhpt 0:c25c4b67b6a1 175 {
yaaqobhpt 0:c25c4b67b6a1 176 char write_buffer[2];
yaaqobhpt 0:c25c4b67b6a1 177 char read_buffer[4];
yaaqobhpt 0:c25c4b67b6a1 178
yaaqobhpt 0:c25c4b67b6a1 179 write_buffer[0] = 0xA0;
yaaqobhpt 0:c25c4b67b6a1 180 i2c.write(addr8bit, write_buffer, 1); wait_us(100);
yaaqobhpt 0:c25c4b67b6a1 181 i2c.read( addr8bit, read_buffer, 4);
yaaqobhpt 0:c25c4b67b6a1 182 countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
yaaqobhpt 0:c25c4b67b6a1 183 countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
yaaqobhpt 0:c25c4b67b6a1 184 }
yaaqobhpt 0:c25c4b67b6a1 185
yaaqobhpt 0:c25c4b67b6a1 186 void getCountsAndReset()
yaaqobhpt 0:c25c4b67b6a1 187 {
yaaqobhpt 0:c25c4b67b6a1 188 char write_buffer[2];
yaaqobhpt 0:c25c4b67b6a1 189 char read_buffer[4];
yaaqobhpt 0:c25c4b67b6a1 190
yaaqobhpt 0:c25c4b67b6a1 191 write_buffer[0] = 0xA4;
yaaqobhpt 0:c25c4b67b6a1 192 i2c.write(addr8bit, write_buffer, 1); wait_us(100);
yaaqobhpt 0:c25c4b67b6a1 193 i2c.read( addr8bit, read_buffer, 4);
yaaqobhpt 0:c25c4b67b6a1 194 countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
yaaqobhpt 0:c25c4b67b6a1 195 countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
yaaqobhpt 0:c25c4b67b6a1 196 }