Gonçalo Lopes / Mbed OS 3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BufferedSerial

Robot.cpp@5:25bd866ef068, 2021-05-17 (annotated)

Committer:
xaficz
Date:
Mon May 17 15:03:21 2021 +0000
Revision:
5:25bd866ef068
Parent:
1:dc87724abce8
Child:
6:df6b8b2468d8
4_real

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]));
xaficz 5:25bd866ef068 63 }
xaficz 5:25bd866ef068 64
xaficz 5:25bd866ef068 65 //Funcao para criar um mapa simples com um quadrado no meio
xaficz 5:25bd866ef068 66 void read_map(int Map_Matrix[80][80]){
xaficz 5:25bd866ef068 67 for(int i = 0; i < 80; i++){
xaficz 5:25bd866ef068 68 for(int j = 0; j < 80; j++){
xaficz 5:25bd866ef068 69 if(i == 0 || j==0 || i==79 || j==79){
xaficz 5:25bd866ef068 70 Map_Matrix[i][j] = 1;
xaficz 5:25bd866ef068 71 }
xaficz 5:25bd866ef068 72 else{
xaficz 5:25bd866ef068 73 Map_Matrix[i][j] = 0;
xaficz 5:25bd866ef068 74 if(i >= 10 && j>=10 && i<=69 && j<=69){
xaficz 5:25bd866ef068 75 Map_Matrix[i][j] = 1;
xaficz 5:25bd866ef068 76 }
xaficz 5:25bd866ef068 77 }
xaficz 5:25bd866ef068 78 }
xaficz 5:25bd866ef068 79 }
xaficz 5:25bd866ef068 80 }
xaficz 5:25bd866ef068 81