João Pedro Castilho / Mbed OS Lab4

Dependencies:   BufferedSerial

Robot.cpp@0:0d3a25d4697e, 2021-05-27 (annotated)

Committer:
ccpjboss
Date:
Thu May 27 08:53:19 2021 +0000
Revision:
0:0d3a25d4697e
tet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ccpjboss 0:0d3a25d4697e 1 #include "Robot.h"
ccpjboss 0:0d3a25d4697e 2 #include "mbed.h"
ccpjboss 0:0d3a25d4697e 3
ccpjboss 0:0d3a25d4697e 4 I2C i2c(I2C_SDA, I2C_SCL );
ccpjboss 0:0d3a25d4697e 5 const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
ccpjboss 0:0d3a25d4697e 6
ccpjboss 0:0d3a25d4697e 7 int16_t countsLeft = 0;
ccpjboss 0:0d3a25d4697e 8 int16_t countsRight = 0;
ccpjboss 0:0d3a25d4697e 9
ccpjboss 0:0d3a25d4697e 10 void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
ccpjboss 0:0d3a25d4697e 11 {
ccpjboss 0:0d3a25d4697e 12 char buffer[5];
ccpjboss 0:0d3a25d4697e 13
ccpjboss 0:0d3a25d4697e 14 buffer[0] = 0xA1;
ccpjboss 0:0d3a25d4697e 15 memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed));
ccpjboss 0:0d3a25d4697e 16 memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed));
ccpjboss 0:0d3a25d4697e 17
ccpjboss 0:0d3a25d4697e 18 i2c.write(addr8bit, buffer, 5); // 5 bytes
ccpjboss 0:0d3a25d4697e 19 }
ccpjboss 0:0d3a25d4697e 20
ccpjboss 0:0d3a25d4697e 21 void setLeftSpeed(int16_t speed)
ccpjboss 0:0d3a25d4697e 22 {
ccpjboss 0:0d3a25d4697e 23 char buffer[3];
ccpjboss 0:0d3a25d4697e 24
ccpjboss 0:0d3a25d4697e 25 buffer[0] = 0xA2;
ccpjboss 0:0d3a25d4697e 26 memcpy(&buffer[1], &speed, sizeof(speed));
ccpjboss 0:0d3a25d4697e 27
ccpjboss 0:0d3a25d4697e 28 i2c.write(addr8bit, buffer, 3); // 3 bytes
ccpjboss 0:0d3a25d4697e 29 }
ccpjboss 0:0d3a25d4697e 30
ccpjboss 0:0d3a25d4697e 31 void setRightSpeed(int16_t speed)
ccpjboss 0:0d3a25d4697e 32 {
ccpjboss 0:0d3a25d4697e 33 char buffer[3];
ccpjboss 0:0d3a25d4697e 34
ccpjboss 0:0d3a25d4697e 35 buffer[0] = 0xA3;
ccpjboss 0:0d3a25d4697e 36 memcpy(&buffer[1], &speed, sizeof(speed));
ccpjboss 0:0d3a25d4697e 37
ccpjboss 0:0d3a25d4697e 38 i2c.write(addr8bit, buffer, 3); // 3 bytes
ccpjboss 0:0d3a25d4697e 39 }
ccpjboss 0:0d3a25d4697e 40
ccpjboss 0:0d3a25d4697e 41 void getCounts()
ccpjboss 0:0d3a25d4697e 42 {
ccpjboss 0:0d3a25d4697e 43 char write_buffer[2];
ccpjboss 0:0d3a25d4697e 44 char read_buffer[4];
ccpjboss 0:0d3a25d4697e 45
ccpjboss 0:0d3a25d4697e 46 write_buffer[0] = 0xA0;
ccpjboss 0:0d3a25d4697e 47 i2c.write(addr8bit, write_buffer, 1); wait_us(100);
ccpjboss 0:0d3a25d4697e 48 i2c.read( addr8bit, read_buffer, 4);
ccpjboss 0:0d3a25d4697e 49 countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
ccpjboss 0:0d3a25d4697e 50 countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
ccpjboss 0:0d3a25d4697e 51 }
ccpjboss 0:0d3a25d4697e 52
ccpjboss 0:0d3a25d4697e 53 void getCountsAndReset()
ccpjboss 0:0d3a25d4697e 54 {
ccpjboss 0:0d3a25d4697e 55 char write_buffer[2];
ccpjboss 0:0d3a25d4697e 56 char read_buffer[4];
ccpjboss 0:0d3a25d4697e 57
ccpjboss 0:0d3a25d4697e 58 write_buffer[0] = 0xA4;
ccpjboss 0:0d3a25d4697e 59 i2c.write(addr8bit, write_buffer, 1); wait_us(100);
ccpjboss 0:0d3a25d4697e 60 i2c.read( addr8bit, read_buffer, 4);
ccpjboss 0:0d3a25d4697e 61 countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
ccpjboss 0:0d3a25d4697e 62 countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
ccpjboss 0:0d3a25d4697e 63 }