sra-romi
Dependencies: BufferedSerial Matrix
Robot.cpp
- Committer:
- joaopsousa99
- Date:
- 2021-05-11
- Revision:
- 4:1defb279922a
- Parent:
- 1:dc87724abce8
File content as of revision 4:1defb279922a:
#include <math.h> #include "Robot.h" #include "mbed.h" #include "control.h" #include "I2C.h" 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]; buffer[0] = 0xA1; memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed)); memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed)); i2c.write(addr8bit, buffer, 5); // 5 bytes } void setLeftSpeed(int16_t speed) { char buffer[3]; buffer[0] = 0xA2; memcpy(&buffer[1], &speed, sizeof(speed)); i2c.write(addr8bit, buffer, 3); // 3 bytes } void setRightSpeed(int16_t speed) { char buffer[3]; buffer[0] = 0xA3; memcpy(&buffer[1], &speed, sizeof(speed)); i2c.write(addr8bit, buffer, 3); // 3 bytes } void getCounts() { char write_buffer[2]; char read_buffer[4]; write_buffer[0] = 0xA0; i2c.write(addr8bit, write_buffer, 1); wait_us(100); 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])); } void getCountsAndReset() { char write_buffer[2]; char read_buffer[4]; write_buffer[0] = 0xA4; i2c.write(addr8bit, write_buffer, 1); wait_us(100); 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); }