Diff: Robot.cpp

Revision:

4:1defb279922a

Parent:

1:dc87724abce8

diff -r 0a718d139ed1 -r 1defb279922a Robot.cpp
--- a/Robot.cpp	Wed Apr 24 16:00:07 2019 +0000
+++ b/Robot.cpp	Tue May 11 18:10:22 2021 +0000
@@ -1,12 +1,27 @@
+#include <math.h>
 #include "Robot.h"
 #include "mbed.h"
+#include "control.h"
+#include "I2C.h"
 
-I2C i2c(I2C_SDA, I2C_SCL );
+I2C i2c(PB_9, PB_8);
 const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
 
+const int16_t COUNTS_PER_ROTATION = 1440;   // ticks dos encoders por volta do motor
+const int16_t WHEEL_DIAMETER = 7;           // diâmetro das rodas, em centímetros
+const int16_t WHEEL_DISTANCE = 14;
+const float MAX_LIN_VEL = 20;
+const float MIN_LIN_VEL = -20;
+const float MIN_SET_SPEEDS = -100;
+const float MAX_SET_SPEEDS = 100;
+
 int16_t countsLeft = 0;
 int16_t countsRight = 0;
 
+float poseX = 0;
+float poseY = 50;
+float poseTheta = 0;
+
 void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
 {
     char buffer[5];
@@ -60,4 +75,43 @@
     i2c.read( addr8bit, read_buffer, 4);
     countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
     countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
+}
+
+float mapSpeeds(float value){
+    value = (value - MIN_LIN_VEL) * (MAX_SET_SPEEDS - MIN_SET_SPEEDS)/(MAX_LIN_VEL - MIN_LIN_VEL) + MIN_SET_SPEEDS;
+
+    if(value < 35 && value > 0)
+        value = 35;
+    if(value > -35 && value < 0)
+        value = -35;
+    if(value > 150)
+        value = 150;
+    if(value < -150)
+        value = -150;
+
+    return value;
+}
+
+void updatePose(){
+    //printf("início do updatePose; ");
+    getCountsAndReset();
+
+    float Dl = (2 * 3.14 * countsLeft) * (WHEEL_DIAMETER / 2) / COUNTS_PER_ROTATION;
+
+    float Dr = (2 * 3.14 * countsRight) * (WHEEL_DIAMETER / 2) / COUNTS_PER_ROTATION;
+
+    float D = (Dl + Dr) / 2;
+
+    float deltaTheta = (Dr - Dl) / WHEEL_DISTANCE;
+
+    poseX = poseX + D * cos(poseTheta + deltaTheta / 2);
+    poseY = poseY + D * sin(poseTheta + deltaTheta / 2);
+    poseTheta = poseTheta + deltaTheta;
+    poseTheta = atan2(sin(poseTheta), cos(poseTheta));
+    //printf("fim do updatePose; ");
+}
+
+void robotVel2wheelVel(float v, float w, float *wheelSpeeds){
+    wheelSpeeds[0] = (v - (WHEEL_DISTANCE / 2) * w) / (WHEEL_DIAMETER / 2);
+    wheelSpeeds[1] = (v + (WHEEL_DISTANCE / 2) * w) / (WHEEL_DIAMETER / 2);
 }
\ No newline at end of file