sra-romi
Dependencies: BufferedSerial Matrix
Diff: Robot.cpp
- Revision:
- 4:1defb279922a
- Parent:
- 1:dc87724abce8
--- 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