Revision 0:a7324f51348d, committed 2019-03-13

Comitter:

PedroMartins96

Date:

Wed Mar 13 23:04:14 2019 +0000

Child:

1:2716ea33958b

Commit message:

444

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication.cpp	Wed Mar 13 23:04:14 2019 +0000
@@ -0,0 +1,36 @@
+#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	Wed Mar 13 23:04:14 2019 +0000
@@ -0,0 +1,11 @@
+#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
+ 
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MessageBuilder.cpp	Wed Mar 13 23:04:14 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;
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MessageBuilder.h	Wed Mar 13 23:04:14 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_ */
+ 
+ 
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.cpp	Wed Mar 13 23:04:14 2019 +0000
@@ -0,0 +1,224 @@
+#include "Robot.h"
+#include "mbed.h"
+
+Serial PC(SERIAL_TX, SERIAL_RX);
+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;
+float De=0, Dd=0, pi=3.141529, Ldis=14.2, r=6.95/2, theta=0, teta=0, x=50, y=10, D=0, Wee=0, Wdd=0, v=0, w=0;
+
+
+////////////////////////////////////////
+// Movimento para uma pose arbitrária //
+////////////////////////////////////////
+void to_pos(float x2,float y2,float phi2)
+{
+    while(1){
+    float alfa,beta,a=33,b=30;
+    float Kp=0.7,Ka=5,Kb=-2;
+    while(abs(x)<abs(x2)-1 || abs(y)<abs(y2)-1 || teta<=(phi2-0.05) || teta>=(phi2+0.05)) {// Condição de paragem
+        // Controlo linear
+        v=Kp*(sqrt(((x2-x)*(x2-x)+(y2-y)*(y2-y))));
+        
+        alfa=-teta+atan2(y2-y,x2-x);
+        
+        if(abs(v)<1){ 
+            alfa=0;
+            a=0;
+            b=0;
+        }
+        // Verifica se o alpha pertence ao limite entre [-pi/2; pi/2], se não adiciona/remove pi e inverte a velocidade
+        if(alfa<-pi/2){
+            alfa=alfa+pi;
+            v=-v;
+        }
+        else if(alfa>pi/2){
+            alfa=alfa-pi;
+            v=-v;
+        }
+        
+        beta=-teta-alfa+phi2;
+        w=Ka*alfa+Kb*beta;
+        
+        if(alfa==0){
+             w=w+abs(w)/w*4;
+             Kb=3;
+        }
+
+        motion();
+        setSpeeds(Wee+a*abs(v)/v,Wdd+b*abs(v)/v);
+
+        //PC.printf("v=%f w=%f alfa=%f beta=%f teta=%f phi2=%f x=%f y=%f\n", v,w,alfa,beta,teta,phi2,x,y);
+        PC.printf("%f %f\n", x,y);
+        }
+        setSpeeds(0,0); // A condição de paragem verifica-se e para-se o robo
+    }
+}
+
+
+/////////////////////////////////
+// Movimento segundo uma linha //
+/////////////////////////////////
+void to_line(float a,float b,float c)
+{
+    float d=0,alfad=0,phi1=0,alfah=0;
+    float Kd=0.45,Kh=15;
+    while(1){
+        while((abs(x) < 200)&&(abs(y)<200)) {
+            v=100;
+            d=(a*x+b*y+c)/sqrt(a*a+b*b);
+
+            //Controlo proporcional que roda a plataforma na direção da linha
+            alfad=(-Kd)*d;
+            if (abs(alfad)<0.3) alfad=alfad*3;
+            phi1=atan2(-a,b);
+            
+            //Controlo proporcional que ajusta o ângulo de direcionamento
+            alfah=Kh*(phi1-teta);
+            
+            w=alfad+alfah;
+            
+            if(w > 12) w = 12;
+            else if(w<-12) w = -12;
+
+            motion();
+            setSpeeds(Wee,Wdd);
+            //PC.printf("v=%f w=%f alfad=%f alfah=%f teta=%f x=%f y=%f\n", v,w,alfad,alfah,teta,x,y);
+            PC.printf("%f %f\n", x,y);
+        }
+        setSpeeds(0,0);
+    }
+}
+
+
+/////////////////////////////
+// Movimento para um ponto //
+/////////////////////////////
+void to_point(float x1,float y1)
+{
+    while(1) {
+        float phi1=0,aux=0;
+        float Kv=1.5,Ks=30;
+        while((abs(x)<abs(x1)-1)||(abs(y)<abs(y1)-1)||(abs(x)>abs(x1)+1)||(abs(y)>abs(y1)+1)) {
+            v=Kv*(sqrt(((x1-x)*(x1-x)+((y1-y)*(y1-y))))); //Controlo de velocidade -- Quanto mais próximo do ponto objetivo, menor vai ser a velocidade
+            
+            phi1=atan2((y1-y),(x1-x)); // Ângulo entre a posição final e a posição atual
+            
+            aux=phi1-teta;
+            //Verifica se a diferença dos angulos pertence ao limite entre [-pi;pi]
+            if(aux<-pi){
+                aux=aux+2*pi;
+            }
+            else if(aux>pi){
+                aux=aux-2*pi;
+            }
+            
+            //Controlo proporcional
+            w=Ks*(aux);
+            
+            if(w > 12) w = 12;
+            else if(w<-12) w = -12;
+
+            motion();
+            setSpeeds(Wee+28,Wdd+25);
+            //PC.printf("v=%f w=%f phi1=%f x=%f y=%f teta=%f\n", v,w,phi1,x,y,teta);
+            PC.printf("%f %f %f %f\n", x,y,teta, w);
+        }
+        setSpeeds(0,0);
+    }
+
+}
+
+///////////////
+// Odometria //
+///////////////
+void motion()
+{
+    getCountsAndReset();
+    De=((2*pi*r)*countsLeft)/1440;
+    Dd=((2*pi*r)*countsRight)/1440;
+    theta=(Dd-De)/Ldis;
+    
+    D=(De+Dd)/2;
+    
+    if(theta==0) {
+        x=x+D*cos(teta);
+        y=y+D*sin(teta);
+        teta=teta;
+    } else {
+        x = x + D*((sin(theta/2)/(theta/2))*cos(teta+theta/2));
+        y = y + D*((sin(theta/2)/(theta/2))*sin(teta+theta/2));
+        teta=teta+theta;
+    }
+    
+    //Limite entre [-2pi; 2pi]
+    if (teta>=(2*pi)) {
+        teta=teta-2*pi;
+    }
+    if (teta<=(-2*pi)) {
+        teta=teta+2*pi;
+    }
+
+    Wee=(v-(Ldis/2)*w);
+    Wdd=(v+(Ldis/2)*w);
+    //PC.printf("phi1=%f D=%f x=%f y=%f teta=%f\n", w,theta,x,y,teta);
+}
+
+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	Wed Mar 13 23:04:14 2019 +0000
@@ -0,0 +1,60 @@
+/** @file */
+#ifndef ROBOT_H_
+#define ROBOT_H_
+
+#include "mbed.h"
+
+extern int16_t countsLeft;
+extern int16_t countsRight;
+extern float De,Dd,T,pi,L,r,x,y,teta;
+
+/** \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();
+
+void to_line(float a,float b,float c);
+
+void motion();
+
+void pressed();
+
+void to_point(float x1,float y1);
+
+void to_pos(float x2, float y2, float phi2);
+
+#endif /* ROBOT_H_ */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 13 23:04:14 2019 +0000
@@ -0,0 +1,38 @@
+#include "mbed.h"
+#include "Robot.h"
+#include "Communication.h"
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+InterruptIn button(USER_BUTTON);
+
+volatile bool mode=true;
+float x1=40,y1=-40;          // Posição inicial do robo
+float a1=1,b1=-2,c1=4;       // Reta ax + by + c = 0
+float x2=50,y2=50,phi2=pi/2; // Pose arbitrária
+
+void pressed()
+{
+    mode= !mode;
+}
+
+int main()
+{
+    pc.baud(9600);
+    button.fall(&pressed);
+
+    while(1) {
+        if (mode==true) {
+            setSpeeds(0,0);
+
+        } else {
+            setSpeeds(0,0); 
+            
+            motion();
+            wait(1);
+            
+            //to_point(x1,y1);
+            //to_line(a1,b1,c1);
+            to_pos(x2,y2,phi2);
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Mar 13 23:04:14 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file