SRM / Mbed 2 deprecated LabWork2VFH

sda

Dependencies:   mbed

Robot.cpp@12:654ad1fe2951, 2019-04-10 (annotated)

Committer:
jsobiecki
Date:
Wed Apr 10 16:58:05 2019 +0000
Revision:
12:654ad1fe2951
Parent:
0:719ea21609f1 
working version

Who changed what in which revision?

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