Gonçalo Lopes / Mbed 2 deprecated VFH

Dependencies:   mbed

Robot.cpp@0:f623431aed01, 2021-05-13 (annotated)

Committer:
xaficz
Date:
Thu May 13 15:19:19 2021 +0000
Revision:
0:f623431aed01
vfh

Who changed what in which revision?

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