main.cpp

00001 #include "mbed.h"
00002 #include <ros.h>
00003 #include <std_msgs/Empty.h>
00004 #include <std_msgs/Int32.h>
00005 #include <std_msgs/String.h>
00006 #include <mbed_custom_msgs/lidar_msg.h>
00007 
00008 #include "Sensor.h"
00009 #include "Motor.h"
00010 
00011 // Set up serial to pc
00012 Serial pc(SERIAL_TX, SERIAL_RX); 
00013 
00014 // Set up I²C on the STM32 NUCLEO-401RE 
00015 #define addr1   (0x29) 
00016 #define addr2   (0x2A)  
00017 #define addr3   (0x2B)
00018 #define addr4   (0x2C)
00019 
00020 #define S1 PC_8
00021 #define S2 PC_9
00022 #define S3 PC_10
00023 #define S4 PC_11
00024 #define S5 PC_12
00025 #define S6 PD_2
00026 #define S7 PG_2
00027 #define S8 PG_3 
00028 
00029 // VL6180x sensors
00030 Sensor sensor_back(I2C_SDA, I2C_SCL, S1);
00031 Sensor sensor_left(I2C_SDA, I2C_SCL, S3);
00032 Sensor sensor_forward(I2C_SDA, I2C_SCL, S5);
00033 Sensor sensor_right(I2C_SDA, I2C_SCL, S7);
00034 
00035 // Motors
00036 QEI wheel_A (PB_12, PA_4, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
00037 QEI wheel_B (PB_5, PB_3, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
00038 Motor motor_left(PC_6, PB_15, PB_13);
00039 Motor motor_right(PA_15, PC_7, PB_4);
00040 
00041 
00042 void move(string dir, int pulse, int pulse_dir, int left_in1, int left_in2, int right_in1, int right_in2)
00043 {
00044     // Variables to allow for any pulse reading   
00045     float initial_pulse_reading = wheel_B.getPulses();
00046     float current_pulse_reading = wheel_B.getPulses();
00047     pc.printf("FIRST current_pulse_reading = %f\r\n", wheel_B.getPulses());
00048     wait_ms(1000);
00049     
00050     // - is forward on our robot
00051     motor_left.move(left_in1, left_in2);
00052     motor_right.move(right_in1, right_in2);
00053     
00054     // loop for moving forward
00055     if (pulse_dir > 0)
00056     {
00057         while(current_pulse_reading <= (initial_pulse_reading + pulse * pulse_dir))
00058         {
00059             //pc.printf("current_pulse_reading 1= %f\r\n", current_pulse_reading);
00060             current_pulse_reading = wheel_A.getPulses();
00061         }
00062     }
00063     else
00064     {
00065         while(current_pulse_reading >= (initial_pulse_reading + pulse * pulse_dir))
00066         {
00067             //pc.printf("current_pulse_reading 2= %f\r\n", current_pulse_reading);
00068             current_pulse_reading = wheel_A.getPulses();
00069         }
00070     }        
00071 
00072 
00073     wait(2);
00074     motor_left.setSpeed(0);
00075     motor_right.setSpeed(0);
00076 }
00077 
00078 /*
00079 void controlMotor(const std_msgs::Int32& motor_dir)
00080 {
00081     switch(motor_dir.data) {
00082         case 0:
00083             motor_left.stop();
00084             motor_right.stop();
00085             break;
00086             
00087         // Move forward
00088         case 1:
00089             move("forward", -0.5, -0.5, 5691, 1); 
00090             break;
00091     
00092         // Move left
00093         case 2:
00094             move("left", 0.5, -0.5, 3000, -1); 
00095             break;
00096             
00097         // Move right
00098         case 3:
00099             move("right", -0.5, 0.5, 3000, 1); 
00100             break;
00101             
00102         // Reverse
00103         case 4:
00104             move("reverse", 0.5, 0.5, 5691, -1);
00105             break;
00106     }
00107 }
00108 */
00109 
00110 
00111 int main() 
00112 {
00113     /*
00114     ros::NodeHandle  nh;
00115     nh.initNode();
00116     
00117     // ROS publisher for sensor readings
00118     mbed_custom_msgs::lidar_msg lidar_msg;
00119     ros::Publisher lidar_pub("lidar_reading", &lidar_msg);
00120     
00121     // ROS subscriber for motors control
00122     ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
00123 
00124     nh.advertise(lidar_pub);
00125     nh.subscribe(motor_sub);
00126     */
00127 
00128     // load settings onto VL6180X sensors 
00129     sensor_forward.init();
00130     // change default address of sensor 2
00131     sensor_forward.changeAddress(addr2);
00132     
00133     sensor_right.init();
00134     // change default address of sensor 3
00135     sensor_right.changeAddress(addr3);
00136 
00137     sensor_back.init();
00138     // change default address of sensor 4
00139     sensor_back.changeAddress(addr4);
00140     
00141     sensor_left.init();
00142     
00143 
00144     motor_left.setSpeed(0.5f);
00145     motor_right.setSpeed(0.5f);
00146     
00147     motor_left.move(1, 0);
00148     motor_right.move(1, 0);
00149   
00150     while (1)
00151     {   
00152         /*
00153         lidar_msg.sensor_forward = sensor_forward.read();
00154         lidar_msg.sensor_back = sensor_back.read();
00155         lidar_msg.sensor_left = sensor_left.read();
00156         lidar_msg.sensor_right = sensor_right.read();         
00157         lidar_pub.publish(&lidar_msg);
00158         */
00159         
00160         int range;
00161         range = sensor_forward.read();
00162         pc.printf("Range = %d\r\n", range);
00163         
00164         if ( (range < 255) && (range != 0) )
00165         {
00166             //pc.printf("I move left");
00167             move("left", 3000, -1, 0, 1, 1, 0); 
00168         }
00169         else
00170         {
00171             //pc.printf("I move forward");
00172             move("forward", 5691, 1, 1, 0, 1, 0); 
00173         }
00174     
00175         //nh.spinOnce();
00176               
00177         wait_ms(10); 
00178     } 
00179 }
00180 
00181 
00182