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?
User | Revision | Line number | New 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 | } |