Revision 9:d0ef39e209b7, committed 2021-05-20

Comitter:

josefg25

Date:

Thu May 20 16:18:59 2021 +0000

Parent:

8:e7a12614d32b

Commit message:

SRA_update

Changed in this revision

--- a/Robot.cpp	Sun May 16 22:40:21 2021 +0000
+++ b/Robot.cpp	Thu May 20 16:18:59 2021 +0000
@@ -2,27 +2,27 @@
 #include "mbed.h"
 
 
-I2C i2c(I2C_SDA, I2C_SCL);
-const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
+//I2C i2c(I2C_SDA, I2C_SCL);
+//const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
 
-int16_t countsLeft = 0;
-int16_t countsRight = 0;
+//int16_t countsLeft = 0;
+//int16_t countsRight = 0;
 
-struct Robot{
+//struct Robot{
     //Robot properties
-    float worldTrobot[3][3]; 
+//    float worldTrobot[3][3]; 
     //Wheels properties
-    float nWheels;
-    float robotTwheels;
-    float wheels2Twheels;
-    float wheelsPoints;
-    float hWheels;
+//    float nWheels;
+//    float robotTwheels;
+//    float wheels2Twheels;
+//    float wheelsPoints;
+//    float hWheels;
     //Platform properties
-    float platformRadius;
-    float hPlatform;
-}; 
+//    float platformRadius;
+//    float hPlatform;
+//}; 
 
-void getPose(struct Robot Robot,float *pose){
+void getPose(struct *Robot,float *pose){
     pose[0] = Robot.worldTrobot[0][2];
     pose[1] = Robot.worldTrobot[1][2];
     pose[2] = atan2(Robot.worldTrobot[1][0],Robot.worldTrobot[0][0]);
@@ -143,8 +143,9 @@
 
 void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
 {
+    const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
     char buffer[5];
-    
+    I2C i2c(I2C_SDA, I2C_SCL);
     buffer[0] = 0xA1;
     memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed));
     memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed));
@@ -154,8 +155,9 @@
 
 void setLeftSpeed(int16_t speed)
 {
+    const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
     char buffer[3];
-    
+    I2C i2c(I2C_SDA, I2C_SCL);
     buffer[0] = 0xA2;
     memcpy(&buffer[1], &speed, sizeof(speed));
 
@@ -164,8 +166,9 @@
 
 void setRightSpeed(int16_t speed)
 {
+    const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
     char buffer[3];
-    
+    I2C i2c(I2C_SDA, I2C_SCL);
     buffer[0] = 0xA3;
     memcpy(&buffer[1], &speed, sizeof(speed));
 
@@ -174,9 +177,12 @@
 
 void getCounts()
 {
+    const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
+    int16_t countsLeft = 0;
+    int16_t countsRight = 0;
     char write_buffer[2];
     char read_buffer[4];
-    
+    I2C i2c(I2C_SDA, I2C_SCL);
     write_buffer[0] = 0xA0;
     i2c.write(addr8bit, write_buffer, 1); wait_us(100);
     i2c.read( addr8bit, read_buffer, 4);
@@ -186,9 +192,12 @@
 
 void getCountsAndReset()
 {
+    const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
+    int16_t countsLeft = 0;
+    int16_t countsRight = 0;
     char write_buffer[2];
     char read_buffer[4];
-    
+    I2C i2c(I2C_SDA, I2C_SCL);
     write_buffer[0] = 0xA4;
     i2c.write(addr8bit, write_buffer, 1); wait_us(100);
     i2c.read( addr8bit, read_buffer, 4);

--- a/Robot.h	Sun May 16 22:40:21 2021 +0000
+++ b/Robot.h	Thu May 20 16:18:59 2021 +0000
@@ -9,7 +9,19 @@
 extern int16_t countsLeft;
 extern int16_t countsRight;
 
-struct Robot;
+typedef struct{
+    //Robot properties
+    float worldTrobot[3][3]; 
+    //Wheels properties
+    float nWheels;
+    float robotTwheels;
+    float wheels2Twheels;
+    float wheelsPoints;
+    float hWheels;
+    //Platform properties
+    float platformRadius;
+    float hPlatform;
+}Robot; 
 
 void getPose(struct Robot Robot,float *pose);
 

--- a/main.cpp	Sun May 16 22:40:21 2021 +0000
+++ b/main.cpp	Thu May 20 16:18:59 2021 +0000
@@ -39,8 +39,8 @@
 float Y = ceil((float)200/5);
 
 float pose[3];
-float pose_f[3][1];
-float PointDetected[2][1];
+float pose_f[3];
+float PointDetected[2];
 
 //posicao inicial
 float xa = 0;
@@ -139,7 +139,7 @@
         start = clock();
         if(lidar.pollSensorData(&data) == 0) {
             pc.printf("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
-            ComputeLidarPoint(&pose[3], &PointDetected, data.distance, data.angle)
+            ComputeLidarPoint(&pose[], &PointDetected[], data.distance, data.angle)
             occupancy_grid_mapping(&log_mapa, Mapa40, d, pose[2], ceil(xa/5), ceil(ya/5), theta, PointDetected[0], PointDetected[1]);
         }
         end = clock();
@@ -175,7 +175,7 @@
                     start = clock();
                     if(lidar.pollSensorData(&data) == 0) {
                         pc.printf("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
-                        ComputeLidarPoint(&pose, &PointDetected, data.distance, data.angle)
+                        ComputeLidarPoint(&pose[], &PointDetected[], data.distance, data.angle)
                         occupancy_grid_mapping(&log_mapa, Mapa40, d, pose[2], ceil(xa/5), ceil(ya/5), theta, PointDetected[0], PointDetected[1]);
                     }
                     end = clock();