João Sousa / Mbed OS romi-vff

sra-romi

Dependencies:   BufferedSerial Matrix

Robot.cpp@1:dc87724abce8, 2019-04-11 (annotated)

Committer:
fabiofaria
Date:
Thu Apr 11 09:51:28 2019 +0000
Revision:
1:dc87724abce8
Child:
4:1defb279922a
Initial commit.

Who changed what in which revision?

UserRevisionLine numberNew 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 }