Soft robot team
Robot team project
Dependencies: QEI Motordriver ros_lib_melodic
Diff: main.cpp
- Revision:
- 19:56bc8226b967
- Parent:
- 17:8831c676778b
- Child:
- 20:074a3638c702
--- a/main.cpp Sat Nov 30 15:13:50 2019 +0000
+++ b/main.cpp Tue Dec 03 09:39:58 2019 +0000
@@ -3,7 +3,7 @@
#include <std_msgs/Empty.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
-#include <mbed_custom_msgs/lidar_msg.h>
+#include <mbed_custom_msgs/motor_msg.h>
#include <motordriver.h>
#include "QEI.h"
#include <cstdlib>
@@ -43,7 +43,8 @@
float sensor_back_reading;
float sensor_left_reading;
float sensor_right_reading;
-//initialised motor speed
+
+//Initialised motor speed
double speed=0.6;
//used to change control to set distances of continous movement
bool control_type = 1;;
@@ -59,137 +60,148 @@
//Thread function to update sensor readings
void flip() {
- while(1) {
- SerialMutex.lock();
- sensor_forward_reading = sensor_forward.read();
- sensor_back_reading = sensor_back.read();
- sensor_left_reading = sensor_left.read();
- sensor_right_reading = sensor_right.read();
- SerialMutex.unlock();
- thread1.wait(2);
- }
-
+ while(1) {
+ SerialMutex.lock();
+
+ // Sensor readings
+ sensor_forward_reading = sensor_forward.read();
+ sensor_back_reading = sensor_back.read();
+ sensor_left_reading = sensor_left.read();
+ sensor_right_reading = sensor_right.read();
+
+ SerialMutex.unlock();
+ thread1.wait(2);
+ }
}
-// Function to move robot (motor speed, motor speed, pulses to travel)
-void move(float motorA, float motorB, int distance)
+void motor_callback(const mbed_custom_msgs::motor_msg& msg)
{
- float current_pulse_reading = 0;
-
- // - is forward on our robot
- A.speed(motorA);
- B.speed(motorB);
+ motor_option = msg.dir;
+ if( (msg.distance <= 0) or (msg.distance > 120) ) {
+ pulse = 6000;
+ } else {
+ pulse = msg.distance * 200;
+ }
- while(abs(current_pulse_reading) <= distance)
- {
- current_pulse_reading = wheel_A.getPulses();
- //Stops Robot moving forward if front sensor detects
- if (sensor_back_reading <255 and motorA>0 and motorB>0){
- return;
- }
- //Stops robot reversing if back sensors detects
- if (sensor_forward_reading <255 and motorA<0 and motorB<0){
- return;
- }
- }
-}
-
-
-void motor_callback(const std_msgs::Int32& motor_dir)
-{
- motor_option = motor_dir.data;
+ //motor_option = motor_dir.data;
thread2.signal_set(1);
}
-void pulse_callback(const std_msgs::Int32& distance){
+// Function to move robot
+void move(float motorA, float motorB, int distance) {
+ float current_pulse_reading = 0;
+
+ // Set motors speed
+ A.speed(motorA);
+ B.speed(motorB);
- //default value of approx 30cm if no distance is recieved or to great
- if((distance.data <= 0) or (distance.data>120)){
- pulse = 6000;
+ while(abs(current_pulse_reading) <= distance) {
+ current_pulse_reading = wheel_A.getPulses();
+ //Stops Robot moving forward if front sensor detects
+ if (sensor_back_reading <255 and motorA>0 and motorB>0){
+ return;
}
- else {
- pulse = distance.data*200;
+
+ //Stops robot reversing if back sensors detects
+ if (sensor_forward_reading <255 and motorA<0 and motorB<0){
+ return;
+ }
}
-
}
+// Function to set robot direction and distance according to data published on motor_control topic
void control_motor(void) {
- while (1)
- {
- thread2.signal_wait(1);
- switch(motor_option) {
- //Forward 30cm
- case 0://49
- if (control_type == 1){
- move(-speed, -speed, pulse);
- }
- else if (control_type ==0){
- move(-speed, -speed, 50);
- }
- break;
+
+ while (1){
+ thread2.signal_wait(1);
+
+ switch(motor_option) {
+
+ // Do nothing - default value published on motor_contorl topic
+ case 0:
+ break;
- //Left 30 degrees
- case 1://50
- if (control_type == 1 ){
- move(speed, -speed, 896);
- }
- else if (control_type == 0){
- move(speed, -speed, 100);
- }
- break;
+ //Forward for pulse/200cm
+ case 1://49
+ if (control_type == 1){
+ move(-speed, -speed, pulse);
+ }
+ else if (control_type ==0){
+ move(-speed, -speed, 50);
+ }
+ break;
+
+ //Left pulse pulse/200/6.7 degrees
+ case 2://50
+ if (control_type == 1 ){
+ move(speed, -speed, pulse/6.7);
+ }
+ else if (control_type == 0){
+ move(speed, -speed, 100);
+ }
+ break;
- //Right 30 degrees
- case 2://51
- if (control_type == 1 ){
- move(-speed, speed, 896);
- }
- else if (control_type == 0 ){
- move(-speed, speed, 60);
- }
- break;
+ //Right pulse/200/6.7 degrees
+ case 3://51
+ if (control_type == 1 ){
+ move(-speed, speed, pulse/6.7);
+ }
+ else if (control_type == 0 ){
+ move(-speed, speed, 60);
+ }
+ break;
- // Reverse 30cm
- case 3://52
- if (control_type ==1 ){
- move(speed, speed, pulse);
- }
- else if (control_type ==0 ){
- move(speed, speed, 50);
- }
- break;
+ // Reverse for pulse/200cm
+ case 4://52
+ if (control_type ==1 ){
+ move(speed, speed, pulse);
+ }
+ else if (control_type ==0 ){
+ move(speed, speed, 50);
+ }
+ break;
+
+ // Speed up
+ case 5:
+ if (speed < 1.0){
+ speed += 0.2;
+ }
+ break;
+
+ // Speed down
+ case 6:
+ if (speed>0.2){
+ speed -= 0.2;
+ }
+ break;
+
+ // Switch control type
+ case 7:
+ control_type =! control_type;
+ break;
- case 4://speed up
- if (speed<1.0){
- speed += 0.2;
- }
- break;
+ // Turn around
+ case 8:
+ if (control_type == 1 ){
+ move(-speed, speed, 5376);
+ }
+ break;
+
+ // Draw a square
+ case 9:
+ for(int i = 0;i<4;i++){
+ move(-speed, -speed, 6000);
+ move(-speed,speed,896);
+ }
+ break;
+ }
- case 5://speed down
- if (speed>0.2){
- speed -= 0.2;
- }
- break;
-
- case 6: // switch control type
- control_type =! control_type;
- break;
-
- case 7: //turn around
- if (control_type == 1 ){
- move(-speed, speed, 5376);
- break;
-
- case 8: //To show case 8 does nothing
- break;
- }
-
- //Reset speed and pulse count
- A.speed(0);
- B.speed(0);
+ //Reset speed and pulse count
+ A.speed(0);
+ B.speed(0);
- wheel_B.reset();
- wheel_A.reset();
- //nh.spinOnce();
+ wheel_B.reset();
+ wheel_A.reset();
}
}
@@ -199,12 +211,8 @@
nh.initNode();
// ROS subscriber for motors control
- ros::Subscriber<std_msgs::Int32> pulse_sub("distance_setting", &pulse_callback);
- ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &motor_callback);
-
-
+ ros::Subscriber<mbed_custom_msgs::motor_msg> motor_sub("motor_control", &motor_callback);
nh.subscribe(motor_sub);
- nh.subscribe(pulse_sub);
// load settings onto VL6180X sensors
sensor_forward.init();
@@ -225,11 +233,8 @@
thread2.start(callback(&control_motor));
while(1) {
- nh.spinOnce();
-
+ nh.spinOnce();
}
-
-
}