Revision 13:0be39d0abac7, committed 2019-11-15

Comitter:

florine_van

Date:

Fri Nov 15 15:27:09 2019 +0000

Parent:

12:0fa4c5a86c75

Commit message:

code to be improved

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Fri Nov 15 15:27:09 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- 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); 
     } 

--- a/motor/Motor.cpp	Fri Nov 15 13:23:56 2019 +0000
+++ b/motor/Motor.cpp	Fri Nov 15 15:27:09 2019 +0000
@@ -1,12 +1,14 @@
 #include "mbed.h"
+#include <string>
 
 #include "Motor.h"
+#include "QEI.h"
 
 ///////////////////////////////////////////////////////////////////
 // Constructor
 ///////////////////////////////////////////////////////////////////
 Motor::Motor(PinName in1, PinName in2, PinName pwm)
-: in1(in1), in2(in2), pwm(pwm) 
+: in1(in1), in2(in2), pwm(pwm)
 {
     this->pwm.period_ms(10);
 }
@@ -14,25 +16,18 @@
 ///////////////////////////////////////////////////////////////////
 // Public methods
 ///////////////////////////////////////////////////////////////////
-void Motor::stop()
-{
-    in1 = 0;
-    in2 = 0;
-}
-
-void Motor::moveForward()
-{
-    in1 = 1; 
-    in2 = 0;    
-}
-
-void Motor::moveBackward()
-{
-    in1 = 0; 
-    in2 = 1;
-}
-
 void Motor::setSpeed(float speed)
 {
     pwm.write(speed);
 }
+
+void Motor::stop()
+{
+    setSpeed(0);
+}
+
+void Motor::move(int in1, int in2)
+{
+    this->in1 = in1; 
+    this->in2 = in2;  
+}
\ No newline at end of file

--- a/motor/Motor.h	Fri Nov 15 13:23:56 2019 +0000
+++ b/motor/Motor.h	Fri Nov 15 15:27:09 2019 +0000
@@ -1,4 +1,7 @@
 #include "mbed.h"
+#include <string>
+
+#include "QEI.h"
 
 class Motor{
     
@@ -15,27 +18,19 @@
     /** Stop the motor
     *
     */
-    void stop();
-    
-    /** Move forward
-    *
-    */
-    void moveForward();
-    
-    /** Move back
-    *
-    */
-    void moveBackward();
+    void stop();  
     
     /** Set the speed
     *
     *   @param speed value to set
     */
     void setSpeed(float speed);
-   
+    
+    
+    void Motor::move(int in1, int in2);
+
 private:
     DigitalOut in1;
     DigitalOut in2;
-    PwmOut pwm;    
-  
+    PwmOut pwm;
 };
\ No newline at end of file