Revision 0:a4a02499a5f3, committed 2020-10-26

Comitter:

jazulienux

Date:

Mon Oct 26 22:25:16 2020 +0000

Commit message:

Robsonema - NucleoFM

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kicker/Kicker.cpp	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,31 @@
+#ifndef KICKER_CPP
+#define KICKER_CPP
+#include "Kicker.h"
+#include <mbed.h>
+#include "config.h"
+
+DigitalOut * selenoid;
+DigitalOut * charge;
+
+Kicker::Kicker()
+{
+    selenoid = new DigitalOut(PORT_SELENOID);
+    charge = new DigitalOut(PORT_CHARGE);
+    selenoid->write(0);
+    charge->write(1);
+}
+
+void Kicker::kicker(float kick_speed)
+{
+    if(kick_speed != 0) {
+        charge->write(0);
+        wait_ms(200);
+        selenoid->write(1);
+        wait_ms(kick_speed);
+        selenoid->write(0);
+        wait_ms(100);
+        charge->write(1);
+    }
+}
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kicker/Kicker.h	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,12 @@
+#ifndef KICKER_H
+#define KICKER_H
+
+class Kicker
+{
+public:
+    Kicker();
+    void kicker(float kick_speed);
+};
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor/Motor.cpp	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,33 @@
+#ifndef MOTOR_CPP
+#define MOTOR_CPP
+#include "Motor.h"
+#include <mbed.h>
+#include "config.h"
+
+I2C * motor;
+
+Motor::Motor()
+{
+    motor = new I2C(SDA_MOTOR,SCL_MOTOR);
+}
+
+void Motor::transmit_i2cmotor(float flpwm,float frpwm,float blpwm,float brpwm)
+{
+    msg_float[0].data = flpwm;
+    msg_float[1].data = frpwm;
+    msg_float[2].data = blpwm;
+    msg_float[3].data = brpwm;
+    motor->write(ADDRES_FLPWM,msg_float[0].m_char,sizeof(msg_float[0].data) + 1);
+    motor->write(ADDRES_FRPWM,msg_float[0].m_char,sizeof(msg_float[1].data) + 1);
+}
+
+void Motor::read_i2cmotor(){
+    RPM[0].data = 0;
+    RPM[1].data = 0;
+    RPM[2].data = 0;
+    RPM[3].data = 0;
+    motor->read(ADDRES_FLPWM,RPM[0].m_char,sizeof(RPM[0].data) + 1);
+}
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor/Motor.h	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,17 @@
+#ifndef MOTOR_H
+#define MOTOR_H
+
+class Motor
+{
+public:
+    Motor();
+    void transmit_i2cmotor(float flpwm,float frpwm,float blpwm,float brpwm);
+    void read_i2cmotor();
+    union Float {
+        float data;
+        char m_char[sizeof(data)];
+    } msg_float[4], RPM[4];
+
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Proximity/Proximity.cpp	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,27 @@
+#ifndef PROXIMITY_CPP
+#define PROXIMITY_CPP
+#include "Proximity.h"
+#include <mbed.h>
+#include "config.h"
+
+DigitalIn * prox_left;
+DigitalIn * prox_right;
+
+Proximity::Proximity()
+{
+    prox_left = new DigitalIn(PORT_PROX_LEFT);
+    prox_right = new DigitalIn(PORT_PROX_RIGHT);
+}
+
+int Proximity::read(){
+    int status = 0;
+    if(prox_left->read() == 0){
+        status += 1;
+    }
+    if(prox_right->read() == 0){
+        status += 2;
+    }
+    return status;
+}
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Proximity/Proximity.h	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,12 @@
+#ifndef PROXIMITY_H
+#define PROXIMITY_H
+
+class Proximity
+{
+public:
+    Proximity();
+    int read();
+};
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Roslib/Roslib.cpp	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,65 @@
+#ifndef ROSLIB_CPP
+#define ROSLIB_CPP
+#include <ros.h>
+#include <std_msgs/Int16.h>
+#include <robsonema_service/Service_Kicker.h>
+#include <robsonema_service/Service_Setpoint.h>
+#include <geometry_msgs/Quaternion.h>
+#include "Roslib.h"
+#include "Kicker.h"
+#include "Motor.h"
+
+Kicker ros_kick;
+Motor ros_motor;
+
+ros::NodeHandle  nh;
+using robsonema_service::Service_Kicker;
+using robsonema_service::Service_Setpoint;
+
+std_msgs::Int16 proximity_msgs;
+ros::Publisher proximity_publish("/robot/proximity_raw", &proximity_msgs);
+
+
+geometry_msgs::Quaternion rpm;
+ros::Publisher rpm_publish("/robot/rpm_raw", &rpm);
+
+void kicker_callback(const Service_Kicker::Request & req, Service_Kicker::Response & res)
+{
+    ros_kick.kicker(req.kick_speed.data);
+}
+
+void setpoint_callback(const Service_Setpoint::Request & req, Service_Setpoint::Response & res)
+{
+    ros_motor.transmit_i2cmotor(req.setpoint.x,req.setpoint.y,req.setpoint.z,req.setpoint.w);
+}
+
+ros::ServiceServer<Service_Kicker::Request, Service_Kicker::Response> server_kicker("/robot/kicker_service", &kicker_callback);
+ros::ServiceServer<Service_Setpoint::Request, Service_Setpoint::Response> server_setpoint("/robot/setpoint_service", &setpoint_callback);
+
+Roslib::Roslib()
+{
+    nh.initNode();
+    nh.advertise(proximity_publish);
+    nh.advertise(rpm_publish);
+    nh.advertiseService(server_kicker);
+    nh.advertiseService(server_setpoint);
+}
+
+void Roslib::publish_data_proximity(float data)
+{
+    proximity_msgs.data = data;
+    proximity_publish.publish(&proximity_msgs);
+}
+
+void Roslib::ros_routine()
+{
+    ros_motor.read_i2cmotor();
+    rpm.x = ros_motor.RPM[0].data;
+    rpm.y = 0;
+    rpm.z = 0;
+    rpm.w = 0;
+    rpm_publish.publish(&rpm);
+    nh.spinOnce();
+}
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Roslib/Roslib.h	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,12 @@
+#ifndef ROSLIB_H
+#define ROSLIB_H
+
+class Roslib
+{
+public:
+    Roslib();
+    void ros_routine();
+    void publish_data_proximity(float data);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Roslib/robsonema_service/Service_Kicker.h	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,76 @@
+#ifndef _ROS_SERVICE_Service_Kicker_h
+#define _ROS_SERVICE_Service_Kicker_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Float32.h"
+
+namespace robsonema_service
+{
+
+static const char SERVICE_KICKER[] = "robsonema_service/Service_Kicker";
+
+  class Service_KickerRequest : public ros::Msg
+  {
+    public:
+      typedef std_msgs::Float32 _kick_speed_type;
+      _kick_speed_type kick_speed;
+
+    Service_KickerRequest():
+      kick_speed()
+    {
+    }
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->kick_speed.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->kick_speed.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return SERVICE_KICKER; };
+    const char * getMD5(){ return "867966d58be95fe06d929f8b7df2e39f"; };
+
+  };
+
+  class Service_KickerResponse : public ros::Msg
+  {
+    public:
+
+    Service_KickerResponse()
+    {
+    }
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    const char * getType(){ return SERVICE_KICKER; };
+    const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class Service_Kicker {
+    public:
+    typedef Service_KickerRequest Request;
+    typedef Service_KickerResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Roslib/robsonema_service/Service_Setpoint.h	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,76 @@
+#ifndef _ROS_SERVICE_Service_Setpoint_h
+#define _ROS_SERVICE_Service_Setpoint_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "geometry_msgs/Quaternion.h"
+
+namespace robsonema_service
+{
+
+static const char SERVICE_SETPOINT[] = "robsonema_service/Service_Setpoint";
+
+  class Service_SetpointRequest : public ros::Msg
+  {
+    public:
+      typedef geometry_msgs::Quaternion _setpoint_type;
+      _setpoint_type setpoint;
+
+    Service_SetpointRequest():
+      setpoint()
+    {
+    }
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->setpoint.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->setpoint.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return SERVICE_SETPOINT; };
+    const char * getMD5(){ return "f79aadd993d02e3313c7a2909b0f4702"; };
+
+  };
+
+  class Service_SetpointResponse : public ros::Msg
+  {
+    public:
+
+    Service_SetpointResponse()
+    {
+    }
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    const char * getType(){ return SERVICE_SETPOINT; };
+    const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class Service_Setpoint {
+    public:
+    typedef Service_SetpointRequest Request;
+    typedef Service_SetpointResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Roslib/ros_lib_melodic.lib	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/garyservin/code/ros_lib_melodic/#da82487f547e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/config/config.h	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,17 @@
+#ifndef CONFIG_H
+#define CONFIG_H
+#include <mbed.h>
+
+#define PORT_PROX_LEFT      PE_5
+#define PORT_PROX_RIGHT     PE_6
+
+#define PORT_SELENOID       PE_2
+#define PORT_CHARGE         PE_4
+
+#define SDA_MOTOR           D14
+#define SCL_MOTOR           D15
+
+#define ADDRES_FLPWM        0xA0
+#define ADDRES_FRPWM        0xA2
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,15 @@
+#include <mbed.h>
+#include "Proximity.h"
+#include "Roslib.h"
+
+Roslib ros;
+Proximity prox;
+
+int main()
+{
+    while(true) {
+        ros.publish_data_proximity(prox.read());
+        ros.ros_routine();
+        wait_ms(10);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file