Diff: main.cpp

Revision:

14:08047fde54f6

Parent:

11:35809512ec11

Child:

15:7c9bf41dd187

--- a/main.cpp	Wed Nov 13 15:59:06 2019 +0000
+++ b/main.cpp	Tue Nov 19 12:42:43 2019 +0000
@@ -4,13 +4,17 @@
 #include <std_msgs/Int32.h>
 #include <std_msgs/String.h>
 #include <mbed_custom_msgs/lidar_msg.h>
-
+#include <motordriver.h>
+#include "QEI.h"
+#include <cstdlib>
 #include "Sensor.h"
-#include "Motor.h"
 
 // Set up serial to pc
 //Serial pc(SERIAL_TX, SERIAL_RX); 
 
+Thread thread1;
+Mutex SerialMutex;
+        
 // Set up I²C on the STM32 NUCLEO-401RE 
 #define addr1   (0x29) 
 #define addr2   (0x2A)  
@@ -26,49 +30,132 @@
 #define S7 PG_2
 #define S8 PG_3 
 
+/*Ticker Sensor_readings()
+rangeForward = sensor_forward.read();
+*/
+
 // VL6180x sensors
 Sensor sensor_back(I2C_SDA, I2C_SCL, S1);
 Sensor sensor_left(I2C_SDA, I2C_SCL, S3);
 Sensor sensor_forward(I2C_SDA, I2C_SCL, S5);
 Sensor sensor_right(I2C_SDA, I2C_SCL, S7);
 
-// Motors
+float sensor_forward_reading;
+float sensor_back_reading;
+float sensor_left_reading;
+float sensor_right_reading;
+
+// Set motorpins for driving
+Motor A(PB_13, PB_15, PC_6, 1); // pwm, fwd, rev, can brake
+Motor B(PB_4, PC_7, PA_15, 1); // pwm, fwd, rev, can brake
 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);
+
+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);
+        }
+
+}
+
+void move(float motorA, float motorB, int distance)
+{
+    // Variables to allow for any pulse reading
+    float current_pulse_reading = 0;
+    
+    // - is forward on our robot
+    A.speed(motorA);
+    B.speed(motorB);
+    // loop for moving forward
+        while(abs(current_pulse_reading) <= distance)
+        {
+            //pc.printf("current_pulse_reading 1= %f\r\n", current_pulse_reading);
+            current_pulse_reading = wheel_A.getPulses();
+            //SerialMutex.lock();
+            //pc.printf("forward = %f, back = %f\n\r", sensor_forward_reading, sensor_back_reading);
+            //SerialMutex.unlock();
+            if (sensor_back_reading <255 and motorA>0 and motorB>0){
+                return;
+                }
+            if (sensor_forward_reading <255 and motorA<0 and motorB<0){
+                return;
+                }
+        }
+        
+    A.speed(0);
+    B.speed(0);
+    
+    wheel_B.reset();
+    wheel_A.reset();
+
+}
+
 
 void controlMotor(const std_msgs::Int32& motor_dir)
 {
     switch(motor_dir.data) {
-        // Stop motor
-        case 0:
-            motor_left.moveForward(0);
-            motor_right.moveForward(0);
-            break;
-            
-        // Move forward
-        case 1:
-            motor_left.moveForward(5691);
-            motor_right.moveForward(5691);
+ 
+        case 0://49
+            move(-0.5, -0.5, 10000); 
             break;
     
         // Move left
-        case 2:
-            motor_left.moveLeft(0.5);
-            motor_right.moveLeft(-0.5);      
+        case 1://50
+            move(0.5, -0.5, 5000); 
             break;
             
         // Move right
-        case 3:
-            motor_left.moveRight(-0.5);
-            motor_right.moveBackward(0.5);
+        case 2://51
+            move(-0.5, 0.5, 5000); 
+            break;
+            
+        // Reverse
+        case 3://52
+            move(0.5, 0.5, 10000);
             break;
+            
+        case default:
+            A.speed(0);
+            B.speed(0);
     }
+    A.speed(0);
+    B.speed(0);
+//    switch(motor_dir.data) {
+//        case 0:
+//            motor_left.stop();
+//            motor_right.stop();
+//            break;
+//            
+//        // Move forward
+//        case 1:
+//            move("forward", -0.5, -0.5); 
+//            break;
+//    
+//        // Move left
+//        case 2:
+//            move("left", 0.5, -0.5, 3000, -1); 
+//            break;
+//            
+//        // Move right
+//        case 3:
+//            move("right", -0.5, 0.5, 3000, 1); 
+//            break;
+//            
+//        // Reverse
+//        case 4:
+//            move("reverse", 0.5, 0.5, 5691, -1);
+//            break;
+//    }
 }
 
-// ROS subscriber for motors control
-ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
+
+
 
 
 int main() 
@@ -76,13 +163,11 @@
     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);
+    // ROS subscriber for motors control
+    ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
 
-    nh.advertise(lidar_pub);
     nh.subscribe(motor_sub);
-
+    
     // load settings onto VL6180X sensors 
     sensor_forward.init();
     // change default address of sensor 2
@@ -98,30 +183,16 @@
     
     sensor_left.init();
     
-    //Set Speeds
-    motor_left.setSpeed(0.5f);
-    motor_right.setSpeed(0.5f);
-  
+    thread1.start(callback(&flip));
+    
     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);
-        */
-        
+    {   
         nh.spinOnce();
-              
-        wait_ms(10); 
-    } 
-}
+        wait(1);
+  
+    }
+} 
 
 
 
+