Gonçalo Lopes / Mbed OS 3

VFH com Lidar

Dependencies:   BufferedSerial

Robot.cpp@7:5fa6f21eb739, 2021-05-24 (annotated)

Committer:
xaficz
Date:
Mon May 24 15:32:13 2021 +0000
Revision:
7:5fa6f21eb739
Parent:
6:df6b8b2468d8 
4__zenaga

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
xaficz 6:df6b8b2468d8 4
xaficz 6:df6b8b2468d8 5 #include "BufferedSerial.h"
xaficz 6:df6b8b2468d8 6 #include "rplidar.h"
xaficz 6:df6b8b2468d8 7
xaficz 6:df6b8b2468d8 8 #include "Communication.h"
xaficz 6:df6b8b2468d8 9 #include <string.h>
xaficz 6:df6b8b2468d8 10 #include <stdio.h>
xaficz 6:df6b8b2468d8 11 #include <math.h>
xaficz 6:df6b8b2468d8 12
fabiofaria 1:dc87724abce8 13 I2C i2c(I2C_SDA, I2C_SCL );
fabiofaria 1:dc87724abce8 14 const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
fabiofaria 1:dc87724abce8 15
fabiofaria 1:dc87724abce8 16 int16_t countsLeft = 0;
fabiofaria 1:dc87724abce8 17 int16_t countsRight = 0;
fabiofaria 1:dc87724abce8 18
fabiofaria 1:dc87724abce8 19 void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
fabiofaria 1:dc87724abce8 20 {
fabiofaria 1:dc87724abce8 21 char buffer[5];
fabiofaria 1:dc87724abce8 22
fabiofaria 1:dc87724abce8 23 buffer[0] = 0xA1;
fabiofaria 1:dc87724abce8 24 memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed));
fabiofaria 1:dc87724abce8 25 memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed));
fabiofaria 1:dc87724abce8 26
fabiofaria 1:dc87724abce8 27 i2c.write(addr8bit, buffer, 5); // 5 bytes
fabiofaria 1:dc87724abce8 28 }
fabiofaria 1:dc87724abce8 29
fabiofaria 1:dc87724abce8 30 void setLeftSpeed(int16_t speed)
fabiofaria 1:dc87724abce8 31 {
fabiofaria 1:dc87724abce8 32 char buffer[3];
fabiofaria 1:dc87724abce8 33
fabiofaria 1:dc87724abce8 34 buffer[0] = 0xA2;
fabiofaria 1:dc87724abce8 35 memcpy(&buffer[1], &speed, sizeof(speed));
fabiofaria 1:dc87724abce8 36
fabiofaria 1:dc87724abce8 37 i2c.write(addr8bit, buffer, 3); // 3 bytes
fabiofaria 1:dc87724abce8 38 }
fabiofaria 1:dc87724abce8 39
fabiofaria 1:dc87724abce8 40 void setRightSpeed(int16_t speed)
fabiofaria 1:dc87724abce8 41 {
fabiofaria 1:dc87724abce8 42 char buffer[3];
fabiofaria 1:dc87724abce8 43
fabiofaria 1:dc87724abce8 44 buffer[0] = 0xA3;
fabiofaria 1:dc87724abce8 45 memcpy(&buffer[1], &speed, sizeof(speed));
fabiofaria 1:dc87724abce8 46
fabiofaria 1:dc87724abce8 47 i2c.write(addr8bit, buffer, 3); // 3 bytes
fabiofaria 1:dc87724abce8 48 }
fabiofaria 1:dc87724abce8 49
fabiofaria 1:dc87724abce8 50 void getCounts()
fabiofaria 1:dc87724abce8 51 {
fabiofaria 1:dc87724abce8 52 char write_buffer[2];
fabiofaria 1:dc87724abce8 53 char read_buffer[4];
fabiofaria 1:dc87724abce8 54
fabiofaria 1:dc87724abce8 55 write_buffer[0] = 0xA0;
fabiofaria 1:dc87724abce8 56 i2c.write(addr8bit, write_buffer, 1); wait_us(100);
fabiofaria 1:dc87724abce8 57 i2c.read( addr8bit, read_buffer, 4);
fabiofaria 1:dc87724abce8 58 countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
fabiofaria 1:dc87724abce8 59 countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
fabiofaria 1:dc87724abce8 60 }
fabiofaria 1:dc87724abce8 61
fabiofaria 1:dc87724abce8 62 void getCountsAndReset()
fabiofaria 1:dc87724abce8 63 {
fabiofaria 1:dc87724abce8 64 char write_buffer[2];
fabiofaria 1:dc87724abce8 65 char read_buffer[4];
fabiofaria 1:dc87724abce8 66
fabiofaria 1:dc87724abce8 67 write_buffer[0] = 0xA4;
fabiofaria 1:dc87724abce8 68 i2c.write(addr8bit, write_buffer, 1); wait_us(100);
fabiofaria 1:dc87724abce8 69 i2c.read( addr8bit, read_buffer, 4);
fabiofaria 1:dc87724abce8 70 countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
fabiofaria 1:dc87724abce8 71 countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
xaficz 5:25bd866ef068 72 }