Experiencias do Henrique na quinta/sexta a noite
Dependencies: BufferedSerial
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