Diff: main.cpp

Revision:

13:0be39d0abac7

Parent:

12:0fa4c5a86c75

--- a/main.cpp	Fri Nov 15 13:23:56 2019 +0000
+++ b/main.cpp	Fri Nov 15 15:27:09 2019 +0000
@@ -9,7 +9,7 @@
 #include "Motor.h"
 
 // Set up serial to pc
-//Serial pc(SERIAL_TX, SERIAL_RX); 
+Serial pc(SERIAL_TX, SERIAL_RX); 
 
 // Set up I²C on the STM32 NUCLEO-401RE 
 #define addr1   (0x29) 
@@ -33,9 +33,49 @@
 Sensor sensor_right(I2C_SDA, I2C_SCL, S7);
 
 // 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);
 Motor motor_right(PA_15, PC_7, PB_4);
 
+
+void move(string dir, int pulse, int pulse_dir, int left_in1, int left_in2, int right_in1, int right_in2)
+{
+    // Variables to allow for any pulse reading   
+    float initial_pulse_reading = wheel_B.getPulses();
+    float current_pulse_reading = wheel_B.getPulses();
+    pc.printf("FIRST current_pulse_reading = %f\r\n", wheel_B.getPulses());
+    wait_ms(1000);
+    
+    // - is forward on our robot
+    motor_left.move(left_in1, left_in2);
+    motor_right.move(right_in1, right_in2);
+    
+    // loop for moving forward
+    if (pulse_dir > 0)
+    {
+        while(current_pulse_reading <= (initial_pulse_reading + pulse * pulse_dir))
+        {
+            //pc.printf("current_pulse_reading 1= %f\r\n", current_pulse_reading);
+            current_pulse_reading = wheel_A.getPulses();
+        }
+    }
+    else
+    {
+        while(current_pulse_reading >= (initial_pulse_reading + pulse * pulse_dir))
+        {
+            //pc.printf("current_pulse_reading 2= %f\r\n", current_pulse_reading);
+            current_pulse_reading = wheel_A.getPulses();
+        }
+    }        
+
+
+    wait(2);
+    motor_left.setSpeed(0);
+    motor_right.setSpeed(0);
+}
+
+/*
 void controlMotor(const std_msgs::Int32& motor_dir)
 {
     switch(motor_dir.data) {
@@ -46,26 +86,31 @@
             
         // Move forward
         case 1:
-            motor_left.moveForward();
-            motor_right.moveForward();
+            move("forward", -0.5, -0.5, 5691, 1); 
             break;
     
         // Move left
         case 2:
-            motor_left.moveBackward();
-            motor_right.moveForward();      
+            move("left", 0.5, -0.5, 3000, -1); 
             break;
             
         // Move right
         case 3:
-            motor_left.moveForward();
-            motor_right.moveBackward();
+            move("right", -0.5, 0.5, 3000, 1); 
+            break;
+            
+        // Reverse
+        case 4:
+            move("reverse", 0.5, 0.5, 5691, -1);
             break;
     }
 }
+*/
+
 
 int main() 
 {
+    /*
     ros::NodeHandle  nh;
     nh.initNode();
     
@@ -78,6 +123,7 @@
 
     nh.advertise(lidar_pub);
     nh.subscribe(motor_sub);
+    */
 
     // load settings onto VL6180X sensors 
     sensor_forward.init();
@@ -94,25 +140,39 @@
     
     sensor_left.init();
     
-    //Set Speeds
+
     motor_left.setSpeed(0.5f);
     motor_right.setSpeed(0.5f);
+    
+    motor_left.move(1, 0);
+    motor_right.move(1, 0);
   
     while (1)
-    {        
+    {   
+        /*
         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);
+        */
         
-        /*
         int range;
         range = sensor_forward.read();
         pc.printf("Range = %d\r\n", range);
-        */
+        
+        if ( (range < 255) && (range != 0) )
+        {
+            //pc.printf("I move left");
+            move("left", 3000, -1, 0, 1, 1, 0); 
+        }
+        else
+        {
+            //pc.printf("I move forward");
+            move("forward", 5691, 1, 1, 0, 1, 0); 
+        }
     
-        nh.spinOnce();
+        //nh.spinOnce();
               
         wait_ms(10); 
     }