Henrique Cardoso
/
Lidar_Rodas
Experiencias do Henrique na quinta a noite
Robot.cpp@1:dc87724abce8, 2019-04-11 (annotated)
- Committer:
- fabiofaria
- Date:
- Thu Apr 11 09:51:28 2019 +0000
- Revision:
- 1:dc87724abce8
Initial commit.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fabiofaria | 1:dc87724abce8 | 1 | #include "Robot.h" |
fabiofaria | 1:dc87724abce8 | 2 | #include "mbed.h" |
fabiofaria | 1:dc87724abce8 | 3 | |
fabiofaria | 1:dc87724abce8 | 4 | I2C i2c(I2C_SDA, I2C_SCL ); |
fabiofaria | 1:dc87724abce8 | 5 | const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal). |
fabiofaria | 1:dc87724abce8 | 6 | |
fabiofaria | 1:dc87724abce8 | 7 | int16_t countsLeft = 0; |
fabiofaria | 1:dc87724abce8 | 8 | int16_t countsRight = 0; |
fabiofaria | 1:dc87724abce8 | 9 | |
fabiofaria | 1:dc87724abce8 | 10 | void setSpeeds(int16_t leftSpeed, int16_t rightSpeed) |
fabiofaria | 1:dc87724abce8 | 11 | { |
fabiofaria | 1:dc87724abce8 | 12 | char buffer[5]; |
fabiofaria | 1:dc87724abce8 | 13 | |
fabiofaria | 1:dc87724abce8 | 14 | buffer[0] = 0xA1; |
fabiofaria | 1:dc87724abce8 | 15 | memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed)); |
fabiofaria | 1:dc87724abce8 | 16 | memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed)); |
fabiofaria | 1:dc87724abce8 | 17 | |
fabiofaria | 1:dc87724abce8 | 18 | i2c.write(addr8bit, buffer, 5); // 5 bytes |
fabiofaria | 1:dc87724abce8 | 19 | } |
fabiofaria | 1:dc87724abce8 | 20 | |
fabiofaria | 1:dc87724abce8 | 21 | void setLeftSpeed(int16_t speed) |
fabiofaria | 1:dc87724abce8 | 22 | { |
fabiofaria | 1:dc87724abce8 | 23 | char buffer[3]; |
fabiofaria | 1:dc87724abce8 | 24 | |
fabiofaria | 1:dc87724abce8 | 25 | buffer[0] = 0xA2; |
fabiofaria | 1:dc87724abce8 | 26 | memcpy(&buffer[1], &speed, sizeof(speed)); |
fabiofaria | 1:dc87724abce8 | 27 | |
fabiofaria | 1:dc87724abce8 | 28 | i2c.write(addr8bit, buffer, 3); // 3 bytes |
fabiofaria | 1:dc87724abce8 | 29 | } |
fabiofaria | 1:dc87724abce8 | 30 | |
fabiofaria | 1:dc87724abce8 | 31 | void setRightSpeed(int16_t speed) |
fabiofaria | 1:dc87724abce8 | 32 | { |
fabiofaria | 1:dc87724abce8 | 33 | char buffer[3]; |
fabiofaria | 1:dc87724abce8 | 34 | |
fabiofaria | 1:dc87724abce8 | 35 | buffer[0] = 0xA3; |
fabiofaria | 1:dc87724abce8 | 36 | memcpy(&buffer[1], &speed, sizeof(speed)); |
fabiofaria | 1:dc87724abce8 | 37 | |
fabiofaria | 1:dc87724abce8 | 38 | i2c.write(addr8bit, buffer, 3); // 3 bytes |
fabiofaria | 1:dc87724abce8 | 39 | } |
fabiofaria | 1:dc87724abce8 | 40 | |
fabiofaria | 1:dc87724abce8 | 41 | void getCounts() |
fabiofaria | 1:dc87724abce8 | 42 | { |
fabiofaria | 1:dc87724abce8 | 43 | char write_buffer[2]; |
fabiofaria | 1:dc87724abce8 | 44 | char read_buffer[4]; |
fabiofaria | 1:dc87724abce8 | 45 | |
fabiofaria | 1:dc87724abce8 | 46 | write_buffer[0] = 0xA0; |
fabiofaria | 1:dc87724abce8 | 47 | i2c.write(addr8bit, write_buffer, 1); wait_us(100); |
fabiofaria | 1:dc87724abce8 | 48 | i2c.read( addr8bit, read_buffer, 4); |
fabiofaria | 1:dc87724abce8 | 49 | countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1])); |
fabiofaria | 1:dc87724abce8 | 50 | countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3])); |
fabiofaria | 1:dc87724abce8 | 51 | } |
fabiofaria | 1:dc87724abce8 | 52 | |
fabiofaria | 1:dc87724abce8 | 53 | void getCountsAndReset() |
fabiofaria | 1:dc87724abce8 | 54 | { |
fabiofaria | 1:dc87724abce8 | 55 | char write_buffer[2]; |
fabiofaria | 1:dc87724abce8 | 56 | char read_buffer[4]; |
fabiofaria | 1:dc87724abce8 | 57 | |
fabiofaria | 1:dc87724abce8 | 58 | write_buffer[0] = 0xA4; |
fabiofaria | 1:dc87724abce8 | 59 | i2c.write(addr8bit, write_buffer, 1); wait_us(100); |
fabiofaria | 1:dc87724abce8 | 60 | i2c.read( addr8bit, read_buffer, 4); |
fabiofaria | 1:dc87724abce8 | 61 | countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1])); |
fabiofaria | 1:dc87724abce8 | 62 | countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3])); |
fabiofaria | 1:dc87724abce8 | 63 | } |