Robot.cpp

00001 #include "Robot.h "
00002 #include "mbed.h"
00003 
00004 I2C i2c(I2C_SDA, I2C_SCL );
00005 const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
00006 
00007 int16_t countsLeft = 0;
00008 int16_t countsRight = 0;
00009 
00010 void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
00011 {
00012     char buffer[5];
00013     
00014     buffer[0] = 0xA1;
00015     memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed));
00016     memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed));
00017 
00018     i2c.write(addr8bit, buffer, 5); // 5 bytes
00019 }
00020 
00021 void setLeftSpeed(int16_t speed)
00022 {
00023     char buffer[3];
00024     
00025     buffer[0] = 0xA2;
00026     memcpy(&buffer[1], &speed, sizeof(speed));
00027 
00028     i2c.write(addr8bit, buffer, 3); // 3 bytes
00029 }
00030 
00031 void setRightSpeed(int16_t speed)
00032 {
00033     char buffer[3];
00034     
00035     buffer[0] = 0xA3;
00036     memcpy(&buffer[1], &speed, sizeof(speed));
00037 
00038     i2c.write(addr8bit, buffer, 3); // 3 bytes
00039 }
00040 
00041 void getCounts()
00042 {
00043     char write_buffer[2];
00044     char read_buffer[4];
00045     
00046     write_buffer[0] = 0xA0;
00047     i2c.write(addr8bit, write_buffer, 1); wait_us(100);
00048     i2c.read( addr8bit, read_buffer, 4);
00049     countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
00050     countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
00051 }
00052 
00053 void getCountsAndReset()
00054 {
00055     char write_buffer[2];
00056     char read_buffer[4];
00057     
00058     write_buffer[0] = 0xA4;
00059     i2c.write(addr8bit, write_buffer, 1); wait_us(100);
00060     i2c.read( addr8bit, read_buffer, 4);
00061     countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
00062     countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
00063 }