Diff: main.cpp

Revision:

12:817da876ae2f

Parent:

11:35809512ec11

Child:

13:d41144f89195

--- a/main.cpp	Wed Nov 13 15:59:06 2019 +0000
+++ b/main.cpp	Fri Nov 15 17:38:04 2019 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include <ros.h>
+#include <cstdlib>
 #include <std_msgs/Empty.h>
 #include <std_msgs/Int32.h>
 #include <std_msgs/String.h>
@@ -7,9 +8,7 @@
 
 #include "Sensor.h"
 #include "Motor.h"
-
-// Set up serial to pc
-//Serial pc(SERIAL_TX, SERIAL_RX); 
+#include "QEI.h"
 
 // Set up I²C on the STM32 NUCLEO-401RE 
 #define addr1   (0x29) 
@@ -35,50 +34,77 @@
 // Motors
 QEI wheel_A (PB_12, PA_4, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
 QEI wheel_B (PB_5, PB_3, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
-Motor motor_left(PC_6, PB_15, PB_13, wheel_A);
-Motor motor_right(PA_15, PC_7, PB_4, wheel_B);
+Motor motor_left(PC_6, PB_15, PB_13);
+Motor motor_right(PA_15, PC_7, PB_4);
+
+
+void pulse(int distance)
+{
+    // Variable to allow for any pulse reading   
+    float current_pulse_reading = wheel_B.getPulses();
+    
+    while(abs(current_pulse_reading) <= distance)
+    {
+        current_pulse_reading = wheel_A.getPulses();
+    }       
+
+    motor_left.stop();
+    motor_right.stop();
+    
+    wheel_A.reset();
+    wheel_B.reset();
+}
 
 void controlMotor(const std_msgs::Int32& motor_dir)
 {
     switch(motor_dir.data) {
-        // Stop motor
+        // Stop motors
         case 0:
-            motor_left.moveForward(0);
-            motor_right.moveForward(0);
+            motor_left.stop();
+            motor_right.stop();
             break;
             
         // Move forward
         case 1:
-            motor_left.moveForward(5691);
-            motor_right.moveForward(5691);
+            motor_left.moveForward();
+            motor_right.moveForward();
+            pulse(5691); 
             break;
     
         // Move left
         case 2:
-            motor_left.moveLeft(0.5);
-            motor_right.moveLeft(-0.5);      
+            motor_left.moveBackward();
+            motor_right.moveForward();     
+            pulse(3000); 
             break;
             
         // Move right
         case 3:
-            motor_left.moveRight(-0.5);
-            motor_right.moveBackward(0.5);
+            motor_left.moveForward();
+            motor_right.moveBackward();
+            pulse(3000); 
+            break;
+            
+        // Reverse
+        case 4:
+            motor_left.moveBackward();
+            motor_right.moveBackward();
+            pulse(5691);
             break;
     }
 }
 
-// ROS subscriber for motors control
-ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
-
-
 int main() 
 {
     ros::NodeHandle  nh;
     nh.initNode();
     
     // ROS publisher for sensor readings
-    mbed_custom_msgs::lidar_msg int_lidar_msg;
-    ros::Publisher lidar_pub("lidar_reading", &int_lidar_msg);
+    mbed_custom_msgs::lidar_msg lidar_msg;
+    ros::Publisher lidar_pub("lidar_reading", &lidar_msg);
+    
+    // ROS subscriber for motors control
+    ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
 
     nh.advertise(lidar_pub);
     nh.subscribe(motor_sub);
@@ -87,7 +113,7 @@
     sensor_forward.init();
     // change default address of sensor 2
     sensor_forward.changeAddress(addr2);
-    
+
     sensor_right.init();
     // change default address of sensor 3
     sensor_right.changeAddress(addr3);
@@ -103,20 +129,13 @@
     motor_right.setSpeed(0.5f);
   
     while (1)
-    {        
-        int_lidar_msg.sensor_forward  = sensor_forward.read();
-        int_lidar_msg.sensor_right  = sensor_right.read();
-        int_lidar_msg.sensor_back  = sensor_back.read();
-        int_lidar_msg.sensor_left  = sensor_left.read();         
-
-        lidar_pub.publish(&int_lidar_msg);
-        
-        /*
-        int range;
-        range = sensor_forward.read();
-        pc.printf("Range = %d\r\n", range);
-        */
-        
+    {   
+        lidar_msg.sensor_forward = sensor_forward.read();
+        lidar_msg.sensor_back = sensor_back.read();
+        lidar_msg.sensor_left = sensor_left.read();
+        lidar_msg.sensor_right = sensor_right.read();         
+        lidar_pub.publish(&lidar_msg);
+    
         nh.spinOnce();
               
         wait_ms(10);