José Gouveia / Mbed OS SRA2020-2021

feito

Dependencies:   BufferedSerial

Robot.cpp@9:d0ef39e209b7, 2021-05-20 (annotated)

Committer:
josefg25
Date:
Thu May 20 16:18:59 2021 +0000
Revision:
9:d0ef39e209b7
Parent:
7:206792eb84c8 
SRA_update

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