João Sousa / Mbed OS romi-vff

sra-romi

Dependencies:   BufferedSerial Matrix

Robot.cpp@4:1defb279922a, 2021-05-11 (annotated)

Committer:
joaopsousa99
Date:
Tue May 11 18:10:22 2021 +0000
Revision:
4:1defb279922a
Parent:
1:dc87724abce8 
as.djvblaskdvj

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joaopsousa99 4:1defb279922a 1 #include <math.h>
fabiofaria 1:dc87724abce8 2 #include "Robot.h"
fabiofaria 1:dc87724abce8 3 #include "mbed.h"
joaopsousa99 4:1defb279922a 4 #include "control.h"
joaopsousa99 4:1defb279922a 5 #include "I2C.h"
fabiofaria 1:dc87724abce8 6
joaopsousa99 4:1defb279922a 7 I2C i2c(PB_9, PB_8);
fabiofaria 1:dc87724abce8 8 const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
fabiofaria 1:dc87724abce8 9
joaopsousa99 4:1defb279922a 10 const int16_t COUNTS_PER_ROTATION = 1440; // ticks dos encoders por volta do motor
joaopsousa99 4:1defb279922a 11 const int16_t WHEEL_DIAMETER = 7; // diâmetro das rodas, em centímetros
joaopsousa99 4:1defb279922a 12 const int16_t WHEEL_DISTANCE = 14;
joaopsousa99 4:1defb279922a 13 const float MAX_LIN_VEL = 20;
joaopsousa99 4:1defb279922a 14 const float MIN_LIN_VEL = -20;
joaopsousa99 4:1defb279922a 15 const float MIN_SET_SPEEDS = -100;
joaopsousa99 4:1defb279922a 16 const float MAX_SET_SPEEDS = 100;
joaopsousa99 4:1defb279922a 17
fabiofaria 1:dc87724abce8 18 int16_t countsLeft = 0;
fabiofaria 1:dc87724abce8 19 int16_t countsRight = 0;
fabiofaria 1:dc87724abce8 20
joaopsousa99 4:1defb279922a 21 float poseX = 0;
joaopsousa99 4:1defb279922a 22 float poseY = 50;
joaopsousa99 4:1defb279922a 23 float poseTheta = 0;
joaopsousa99 4:1defb279922a 24
fabiofaria 1:dc87724abce8 25 void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
fabiofaria 1:dc87724abce8 26 {
fabiofaria 1:dc87724abce8 27 char buffer[5];
fabiofaria 1:dc87724abce8 28
fabiofaria 1:dc87724abce8 29 buffer[0] = 0xA1;
fabiofaria 1:dc87724abce8 30 memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed));
fabiofaria 1:dc87724abce8 31 memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed));
fabiofaria 1:dc87724abce8 32
fabiofaria 1:dc87724abce8 33 i2c.write(addr8bit, buffer, 5); // 5 bytes
fabiofaria 1:dc87724abce8 34 }
fabiofaria 1:dc87724abce8 35
fabiofaria 1:dc87724abce8 36 void setLeftSpeed(int16_t speed)
fabiofaria 1:dc87724abce8 37 {
fabiofaria 1:dc87724abce8 38 char buffer[3];
fabiofaria 1:dc87724abce8 39
fabiofaria 1:dc87724abce8 40 buffer[0] = 0xA2;
fabiofaria 1:dc87724abce8 41 memcpy(&buffer[1], &speed, sizeof(speed));
fabiofaria 1:dc87724abce8 42
fabiofaria 1:dc87724abce8 43 i2c.write(addr8bit, buffer, 3); // 3 bytes
fabiofaria 1:dc87724abce8 44 }
fabiofaria 1:dc87724abce8 45
fabiofaria 1:dc87724abce8 46 void setRightSpeed(int16_t speed)
fabiofaria 1:dc87724abce8 47 {
fabiofaria 1:dc87724abce8 48 char buffer[3];
fabiofaria 1:dc87724abce8 49
fabiofaria 1:dc87724abce8 50 buffer[0] = 0xA3;
fabiofaria 1:dc87724abce8 51 memcpy(&buffer[1], &speed, sizeof(speed));
fabiofaria 1:dc87724abce8 52
fabiofaria 1:dc87724abce8 53 i2c.write(addr8bit, buffer, 3); // 3 bytes
fabiofaria 1:dc87724abce8 54 }
fabiofaria 1:dc87724abce8 55
fabiofaria 1:dc87724abce8 56 void getCounts()
fabiofaria 1:dc87724abce8 57 {
fabiofaria 1:dc87724abce8 58 char write_buffer[2];
fabiofaria 1:dc87724abce8 59 char read_buffer[4];
fabiofaria 1:dc87724abce8 60
fabiofaria 1:dc87724abce8 61 write_buffer[0] = 0xA0;
fabiofaria 1:dc87724abce8 62 i2c.write(addr8bit, write_buffer, 1); wait_us(100);
fabiofaria 1:dc87724abce8 63 i2c.read( addr8bit, read_buffer, 4);
fabiofaria 1:dc87724abce8 64 countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
fabiofaria 1:dc87724abce8 65 countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
fabiofaria 1:dc87724abce8 66 }
fabiofaria 1:dc87724abce8 67
fabiofaria 1:dc87724abce8 68 void getCountsAndReset()
fabiofaria 1:dc87724abce8 69 {
fabiofaria 1:dc87724abce8 70 char write_buffer[2];
fabiofaria 1:dc87724abce8 71 char read_buffer[4];
fabiofaria 1:dc87724abce8 72
fabiofaria 1:dc87724abce8 73 write_buffer[0] = 0xA4;
fabiofaria 1:dc87724abce8 74 i2c.write(addr8bit, write_buffer, 1); wait_us(100);
fabiofaria 1:dc87724abce8 75 i2c.read( addr8bit, read_buffer, 4);
fabiofaria 1:dc87724abce8 76 countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
fabiofaria 1:dc87724abce8 77 countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
joaopsousa99 4:1defb279922a 78 }
joaopsousa99 4:1defb279922a 79
joaopsousa99 4:1defb279922a 80 float mapSpeeds(float value){
joaopsousa99 4:1defb279922a 81 value = (value - MIN_LIN_VEL) * (MAX_SET_SPEEDS - MIN_SET_SPEEDS)/(MAX_LIN_VEL - MIN_LIN_VEL) + MIN_SET_SPEEDS;
joaopsousa99 4:1defb279922a 82
joaopsousa99 4:1defb279922a 83 if(value < 35 && value > 0)
joaopsousa99 4:1defb279922a 84 value = 35;
joaopsousa99 4:1defb279922a 85 if(value > -35 && value < 0)
joaopsousa99 4:1defb279922a 86 value = -35;
joaopsousa99 4:1defb279922a 87 if(value > 150)
joaopsousa99 4:1defb279922a 88 value = 150;
joaopsousa99 4:1defb279922a 89 if(value < -150)
joaopsousa99 4:1defb279922a 90 value = -150;
joaopsousa99 4:1defb279922a 91
joaopsousa99 4:1defb279922a 92 return value;
joaopsousa99 4:1defb279922a 93 }
joaopsousa99 4:1defb279922a 94
joaopsousa99 4:1defb279922a 95 void updatePose(){
joaopsousa99 4:1defb279922a 96 //printf("início do updatePose; ");
joaopsousa99 4:1defb279922a 97 getCountsAndReset();
joaopsousa99 4:1defb279922a 98
joaopsousa99 4:1defb279922a 99 float Dl = (2 * 3.14 * countsLeft) * (WHEEL_DIAMETER / 2) / COUNTS_PER_ROTATION;
joaopsousa99 4:1defb279922a 100
joaopsousa99 4:1defb279922a 101 float Dr = (2 * 3.14 * countsRight) * (WHEEL_DIAMETER / 2) / COUNTS_PER_ROTATION;
joaopsousa99 4:1defb279922a 102
joaopsousa99 4:1defb279922a 103 float D = (Dl + Dr) / 2;
joaopsousa99 4:1defb279922a 104
joaopsousa99 4:1defb279922a 105 float deltaTheta = (Dr - Dl) / WHEEL_DISTANCE;
joaopsousa99 4:1defb279922a 106
joaopsousa99 4:1defb279922a 107 poseX = poseX + D * cos(poseTheta + deltaTheta / 2);
joaopsousa99 4:1defb279922a 108 poseY = poseY + D * sin(poseTheta + deltaTheta / 2);
joaopsousa99 4:1defb279922a 109 poseTheta = poseTheta + deltaTheta;
joaopsousa99 4:1defb279922a 110 poseTheta = atan2(sin(poseTheta), cos(poseTheta));
joaopsousa99 4:1defb279922a 111 //printf("fim do updatePose; ");
joaopsousa99 4:1defb279922a 112 }
joaopsousa99 4:1defb279922a 113
joaopsousa99 4:1defb279922a 114 void robotVel2wheelVel(float v, float w, float *wheelSpeeds){
joaopsousa99 4:1defb279922a 115 wheelSpeeds[0] = (v - (WHEEL_DISTANCE / 2) * w) / (WHEEL_DIAMETER / 2);
joaopsousa99 4:1defb279922a 116 wheelSpeeds[1] = (v + (WHEEL_DISTANCE / 2) * w) / (WHEEL_DIAMETER / 2);
fabiofaria 1:dc87724abce8 117 }