Revision 1:dc87724abce8, committed 2019-04-11

Comitter:

fabiofaria

Date:

Thu Apr 11 09:51:28 2019 +0000

Parent:

0:2b691d200d6f

Child:

2:e27733eaa594

Commit message:

Initial commit.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication.cpp	Thu Apr 11 09:51:28 2019 +0000
@@ -0,0 +1,35 @@
+#include "Communication.h"
+#include "mbed.h"
+#include "MessageBuilder.h"
+
+const char max_len = 30;
+Serial *serial_object;
+MessageBuilder bin_msg;
+
+void init_communication(Serial *serial_in)
+{
+    serial_object = serial_in;
+}
+
+void write_bytes(char *ptr, unsigned char len)
+{
+    for(int i=0; i<len; i++)
+    {
+        serial_object->putc(ptr[i]);
+    }
+}
+
+void send_odometry(int value1, int value2, int ticks_left, int ticks_right, float x, float y, float theta)
+{
+    bin_msg.reset();
+    bin_msg.add('O');
+    bin_msg.add(value1);
+    bin_msg.add(value2);
+    bin_msg.add(ticks_left);
+    bin_msg.add(ticks_right);
+    bin_msg.add(x);
+    bin_msg.add(y);
+    bin_msg.add(theta);
+
+    write_bytes(bin_msg.message, bin_msg.length());
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication.h	Thu Apr 11 09:51:28 2019 +0000
@@ -0,0 +1,10 @@
+#ifndef COMMUNICATION_H
+#define COMMUNICATION_H
+
+#include "mbed.h"
+
+void init_communication(Serial *serial_in);
+void write_bytes(char *ptr, unsigned char len);
+void send_odometry(int value1, int value2, int ticks_left, int ticks_right, float x, float y, float theta);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MessageBuilder.cpp	Thu Apr 11 09:51:28 2019 +0000
@@ -0,0 +1,77 @@
+#include "MessageBuilder.h"
+#include "mbed.h"
+
+MessageBuilder::MessageBuilder() {
+	reset();
+}
+
+MessageBuilder::~MessageBuilder() {
+	// TODO Auto-generated destructor stub
+}
+
+char MessageBuilder::add(const void* data, size_t len) {
+	if (available() >= len) {
+		memcpy(_pointer, data, len);
+		_pointer += len;
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
+void MessageBuilder::reset() {
+	message[0] = 0x06;
+	message[1] = 0x85;
+	_pointer = &message[2];
+}
+
+// Note: if message size grow beyond 32 bytes, return "size_t" insted, because it
+// is the most appropriate type for "sizeof" operator. Now, unsgined char is used
+// for memory economy.
+unsigned char MessageBuilder::available() {
+	return &message[max_len - 1] - _pointer + 1;
+}
+
+unsigned char MessageBuilder::length() {
+	return _pointer - &message[0];
+}
+
+char MessageBuilder::add(float data) {
+	if (available() >= sizeof(data)) {
+		memcpy(_pointer, &data, sizeof(data));
+		_pointer += sizeof(data);
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
+char MessageBuilder::add(int data) {
+	if (available() >= sizeof(data)) {
+		memcpy(_pointer, &data, sizeof(data));
+		_pointer += sizeof(data);
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
+char MessageBuilder::add(char data) {
+	if (available() >= sizeof(data)) {
+		memcpy(_pointer, &data, sizeof(data));
+		_pointer += sizeof(data);
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
+char MessageBuilder::add(unsigned int data) {
+	if (available() >= sizeof(data)) {
+		memcpy(_pointer, &data, sizeof(data));
+		_pointer += sizeof(data);
+		return 0;
+	} else {
+		return 1;
+	}
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MessageBuilder.h	Thu Apr 11 09:51:28 2019 +0000
@@ -0,0 +1,29 @@
+#ifndef MESSAGEBUILDER_H_
+#define MESSAGEBUILDER_H_
+
+#include "mbed.h"
+
+class MessageBuilder
+{
+private:
+    static const char max_len = 32;
+    char *_pointer;
+
+public:
+    char message[max_len];
+
+    MessageBuilder();
+    virtual ~MessageBuilder();
+    char add(const void* data, size_t len);
+    char add(char data);
+    char add(float data);
+    char add(int data);
+    char add(unsigned int data);
+    void reset();
+    unsigned char available();
+    unsigned char length();
+};
+
+#endif /* MESSAGEBUILDER_H_ */
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.cpp	Thu Apr 11 09:51:28 2019 +0000
@@ -0,0 +1,63 @@
+#include "Robot.h"
+#include "mbed.h"
+
+I2C i2c(I2C_SDA, I2C_SCL );
+const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
+
+int16_t countsLeft = 0;
+int16_t countsRight = 0;
+
+void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
+{
+    char buffer[5];
+    
+    buffer[0] = 0xA1;
+    memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed));
+    memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed));
+
+    i2c.write(addr8bit, buffer, 5); // 5 bytes
+}
+
+void setLeftSpeed(int16_t speed)
+{
+    char buffer[3];
+    
+    buffer[0] = 0xA2;
+    memcpy(&buffer[1], &speed, sizeof(speed));
+
+    i2c.write(addr8bit, buffer, 3); // 3 bytes
+}
+
+void setRightSpeed(int16_t speed)
+{
+    char buffer[3];
+    
+    buffer[0] = 0xA3;
+    memcpy(&buffer[1], &speed, sizeof(speed));
+
+    i2c.write(addr8bit, buffer, 3); // 3 bytes
+}
+
+void getCounts()
+{
+    char write_buffer[2];
+    char read_buffer[4];
+    
+    write_buffer[0] = 0xA0;
+    i2c.write(addr8bit, write_buffer, 1); wait_us(100);
+    i2c.read( addr8bit, read_buffer, 4);
+    countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
+    countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
+}
+
+void getCountsAndReset()
+{
+    char write_buffer[2];
+    char read_buffer[4];
+    
+    write_buffer[0] = 0xA4;
+    i2c.write(addr8bit, write_buffer, 1); wait_us(100);
+    i2c.read( addr8bit, read_buffer, 4);
+    countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
+    countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.h	Thu Apr 11 09:51:28 2019 +0000
@@ -0,0 +1,49 @@
+/** @file */
+#ifndef ROBOT_H_
+#define ROBOT_H_
+
+#include "mbed.h"
+
+extern int16_t countsLeft;
+extern int16_t countsRight;
+
+/** \brief Sets the speed for both motors.
+ *
+ * \param leftSpeed A number from -300 to 300 representing the speed and
+ * direction of the left motor. Values of -300 or less result in full speed
+ * reverse, and values of 300 or more result in full speed forward.
+ * \param rightSpeed A number from -300 to 300 representing the speed and
+ * direction of the right motor. Values of -300 or less result in full speed
+ * reverse, and values of 300 or more result in full speed forward. */
+void setSpeeds(int16_t leftSpeed, int16_t rightSpeed);
+
+/** \brief Sets the speed for the left motor.
+ *
+ * \param speed A number from -300 to 300 representing the speed and
+ * direction of the left motor.  Values of -300 or less result in full speed
+ * reverse, and values of 300 or more result in full speed forward. */
+void setLeftSpeed(int16_t speed);
+
+/** \brief Sets the speed for the right motor.
+ *
+ * \param speed A number from -300 to 300 representing the speed and
+ * direction of the right motor. Values of -300 or less result in full speed
+ * reverse, and values of 300 or more result in full speed forward. */
+void setRightSpeed(int16_t speed);
+
+/*! Returns the number of counts that have been detected from the both
+ * encoders.  These counts start at 0.  Positive counts correspond to forward
+ * movement of the wheel of the Romi, while negative counts correspond
+ * to backwards movement.
+ *
+ * The count is returned as a signed 16-bit integer.  When the count goes
+ * over 32767, it will overflow down to -32768.  When the count goes below
+ * -32768, it will overflow up to 32767. */
+void getCounts();
+
+/*! This function is just like getCounts() except it also clears the
+ *  counts before returning.  If you call this frequently enough, you will
+ *  not have to worry about the count overflowing. */
+void getCountsAndReset();
+
+#endif /* ROBOT_H_ */
\ No newline at end of file

--- a/main.cpp	Tue Apr 09 17:53:31 2019 +0000
+++ b/main.cpp	Thu Apr 11 09:51:28 2019 +0000
@@ -1,39 +1,38 @@
+// Coded by Luís Afonso 11-04-2019
 #include "mbed.h"
 #include "BufferedSerial.h"
 #include "rplidar.h"
+#include "Robot.h"
+#include "Communication.h"
 
-Serial pc(SERIAL_TX, SERIAL_RX, 1000000);
+Serial pc(SERIAL_TX, SERIAL_RX);
 RPLidar lidar;
 BufferedSerial se_lidar(PA_9, PA_10);
 PwmOut rplidar_motor(A3);
 
-
-
 int main()
 {
+    float odomX, odomY, odomTheta;
+    struct RPLidarMeasurement data;
+    
+    pc.baud(115200);
+    init_communication(&pc);
 
-    pc.baud(1000000);//115200);
-    pc.printf("main\n");
-    wait(1);
-
+    // Lidar initialization
     rplidar_motor.period(0.001f);
     lidar.begin(se_lidar);
     lidar.setAngle(0,360);
 
-    
     pc.printf("Program started.\n");
-    
-    
+        
     lidar.startThreadScan();
     
-
     while(1) {
-        struct RPLidarMeasurement data;
         // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
         if(lidar.pollSensorData(&data) == 0)
         {
-            pc.printf("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit);
+            pc.printf("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
         }
        wait(0.1); 
     }
-}
+}
\ No newline at end of file