For coursework of group 3 in SOFT564Z
Dependencies: Motordriver ros_lib_kinetic
Revision 12:82b8fe254222, committed 2020-01-05
- Comitter:
- Jonathan738
- Date:
- Sun Jan 05 15:42:22 2020 +0000
- Parent:
- 11:0b9098ec11c7
- Commit message:
- Added working version of TOF code
Changed in this revision
--- a/Battery_Monitor.cpp Thu Dec 19 00:13:38 2019 +0000 +++ b/Battery_Monitor.cpp Sun Jan 05 15:42:22 2020 +0000 @@ -16,15 +16,14 @@ void Battery_Monitor::battCheck(void) { - float Read_V_Batt = this->V_Batt.read(); + float Read_V_Batt = this->V_Batt.read() * 16.67f; float temp_batLev = Read_V_Batt - Minimum_VBatt; this->Battery_Level = (Maximum_VBatt - Minimum_VBatt) / temp_batLev; - Global_Battery_Level_Mutex.lock(); - Global_Battery_Level = Read_V_Batt; - Global_Battery_Value = this->Battery_Level; + Global_Battery_Value = Read_V_Batt; + Global_Battery_Level = this->Battery_Level; if(Battery_Update_Flag == false) { Battery_Update_Flag = true;
--- a/Battery_Monitor.hpp Thu Dec 19 00:13:38 2019 +0000 +++ b/Battery_Monitor.hpp Sun Jan 05 15:42:22 2020 +0000 @@ -4,7 +4,7 @@ #include "rtos.h" #define Minimum_VBatt 3.6f -#define Maximum_VBatt 5.0f +#define Maximum_VBatt 4.2f #define PWM_Freq 200.0f #ifndef Define_ONCE_Battery_Monitoring
--- a/General.hpp Thu Dec 19 00:13:38 2019 +0000 +++ b/General.hpp Sun Jan 05 15:42:22 2020 +0000 @@ -26,15 +26,16 @@ /******************************************************************************/ /* Defentions of Pins and Topics for ROS and attached peripherals */ /******************************************************************************/ -#define InPut_Topic "/Nucelo/Command" -#define OutPut_Topic "/Nucleo/State" +#define Control_Topic "/Nucleo/cmd_vel" +#define TOF_Topic "/Nucleo/TOF_Ranges" +#define Status_Topic "/Nucleo/Status" #define Serial_Error_Tx SERIAL_TX #define Serial_Error_Rx SERIAL_RX #define ROS_Tx PD_5 #define ROS_Rx PD_6 -#define ROS_BaudRate 57600 +#define ROS_BaudRate 115200 /******************************************************************************/
--- a/Motors.cpp Thu Dec 19 00:13:38 2019 +0000 +++ b/Motors.cpp Sun Jan 05 15:42:22 2020 +0000 @@ -51,27 +51,8 @@ int previousStateB = currentStateB; int nextmoveflagB = 1; -//X is forward, Z is upwards, Y is sideways in this structure. -//Order is X, Y, Z linear, then X, Y, Z rotational, from 1:6. -void cmdvelresponse(const std_msgs::Int32MultiArray& vinput) { - //Extracting the only commands that will be used in the multiarray, and assuming that the int value attached to each point is 100 times - //the intended PWM constant for that movement command (e.g. a value of 50 in msg.data[6] would give a PWM duty cycle of 0.5 for rotation): - float xlin = 0.01 * vinput.data[1]; - float zrot = 0.01 * vinput.data[6]; - //assume rotation needs to be taken care of first, generally, then linear movement - if(zrot != 0) { //assume positive z is clockwise, negative is anticlockwise, A is left, B is right - A.speed(-zrot); - B.speed(zrot); - } else if(xlin != 0) { - A.speed(xlin); - B.speed(xlin); - } else { - A.stop(1); - B.stop(1); - } -} - -void initialize() { +void initialize() +{ //every time a rising or falling edge is detected for the A or B channels, call the encoder counting interrupt for the respective wheel. //this corresponds to the counting program for the encoders for each wheel, given that quadrature encoding is used. encoderAchannel1.rise(&callbackA); @@ -93,16 +74,17 @@ previousStateB = currentStateB; } -void callbackA() { +void callbackA() +{ //////// //insert the 'if X, carry out the rest of this code' debouncing check here, ensure the delay is minimal enough to avoid hindering 3576 measurements per rotation at max speed //////// - + volatile int changeA = 0; channelAstate1 = encoderAchannel1.read(); channelAstate2 = encoderAchannel2.read(); currentStateA = (channelAstate1 << 1) | (channelAstate2); - + if (((currentStateA ^ previousStateA) != INVALID) && (currentStateA != previousStateA)) { //2 bit state. Right hand bit of prev XOR left hand bit of current //gives 0 if clockwise rotation and 1 if counter clockwise rotation. @@ -122,18 +104,19 @@ } } -void callbackB() { +void callbackB() +{ //////// //insert the 'if X, carry out the rest of this code' debouncing check here, ensure the delay is minimal enough to avoid hindering 3576 measurements per rotation at max speed //////// - + //every time this function is called, increment or decrement the encoder count depending on which direction the relevant wheel is moving - + volatile int changeB = 0; channelBstate1 = encoderBchannel1.read(); channelBstate2 = encoderBchannel2.read(); currentStateB = (channelBstate1 << 1) | (channelBstate2); - + if (((currentStateB ^ previousStateB) != INVALID) && (currentStateB != previousStateB)) { //2 bit state. Right hand bit of prev XOR left hand bit of current //gives 0 if clockwise rotation and 1 if counter clockwise rotation. @@ -153,7 +136,8 @@ } } -void move(const std_msgs::Int32& dinput) { +void move(const std_msgs::Int32& dinput) +{ if(nextmoveflagA == 1 && nextmoveflagB == 1) { //import the relevant ROS message and convert it to a usable encoder target float distance = dinput.data; @@ -168,7 +152,8 @@ } } -void tempMove(float distance) { +void tempMove(float distance) +{ if(nextmoveflagA == 1 && nextmoveflagB == 1) { float mtarget = distance/(2*3.142*radius)*benchmark; atarget = ceil(mtarget + aprevtarget); @@ -180,7 +165,8 @@ } } -void rotate(const std_msgs::Int32& rinput) { +void rotate(const std_msgs::Int32& rinput) +{ if(nextmoveflagA == 1 && nextmoveflagB == 1) { //import the relevant ROS message and convert it to usable encoder targets float degrees = rinput.data; @@ -195,7 +181,8 @@ } } -void tempRotate(float degrees) { +void tempRotate(float degrees) +{ if(nextmoveflagA == 1 && nextmoveflagB == 1) { float rtarget = (degrees/(360*3))*9.15*benchmark; atarget = ceil(rtarget + aprevtarget); @@ -207,7 +194,8 @@ } } -void driveMotors() { +void driveMotors() +{ //depending on the direction the wheels have to move, set their pwms to either positive or negative so each motor moves correctly if(atarget > aprevtarget) { apwm = pwm; @@ -241,3 +229,4 @@ aprevtarget = atarget; bprevtarget = btarget; } +
--- a/Motors.hpp Thu Dec 19 00:13:38 2019 +0000 +++ b/Motors.hpp Sun Jan 05 15:42:22 2020 +0000 @@ -8,9 +8,12 @@ #include "Battery_Monitor.hpp" #include "Pins.h" +#include <std_msgs/Int32.h> + #ifndef Define_ONCE_Motors #define Define_ONCE_Motors +void Motor_Handler(); void initialize(); void move(const std_msgs::Int32& input); void rotate(const std_msgs::Int32& input); @@ -19,6 +22,5 @@ void driveMotors(void); void callbackA(void); void callbackB(void); -void cmdvelresponse(const std_msgs::Int32MultiArray& vinput); #endif \ No newline at end of file
--- a/ROS_Handler.cpp Thu Dec 19 00:13:38 2019 +0000 +++ b/ROS_Handler.cpp Sun Jan 05 15:42:22 2020 +0000 @@ -24,12 +24,11 @@ /******************************************************************************/ ros::NodeHandle_<HaseHardware> Node_; -void Control_CallBack(const std_msgs::Int32MultiArray& msg); -ros::Subscriber<std_msgs::Int32MultiArray> Control_Subscriber("/cmd_vel", &Control_CallBack); +ros::Subscriber<std_msgs::Float32MultiArray> Control_Subscriber(Control_Topic, &Control_CallBack); std_msgs::String State_msg; std_msgs::String TOF_msg; -ros::Publisher Status_PUB("/Nucleo/State", &State_msg); -ros::Publisher TOF_PUB("/Nucleo/TOF_data", &TOF_msg); +ros::Publisher Status_PUB(Status_Topic, &State_msg); +ros::Publisher TOF_PUB(TOF_Topic, &TOF_msg); extern Mutex Global_Battery_Level_Mutex; extern float Global_Battery_Level; @@ -40,8 +39,14 @@ extern bool TOF_Range_Flag; extern Mutex Global_TOF_Range_Mutex; +float LIN_Val, ANG_Val; +Mutex Duty_Mutex; + // Main function for thread to handle ROS comms void ROS_Handler(void){ + LIN_Val = 0; + ANG_Val = 0; + Node_.initNode(); Node_.advertise(Status_PUB); Node_.advertise(TOF_PUB); @@ -56,7 +61,9 @@ Battery_Update_Flag = false; Global_Battery_Level_Mutex.unlock(); char buffer[50]; - sprintf(buffer, "Battery Level : %1.3fV | Battery Voltage %1.3fV", Battery_Level, Battery_Value); + Duty_Mutex.lock(); + sprintf(buffer, "Battery Level : %1.3f% | Battery Voltage %1.3fV | Linear Duty %1.3f | Angular Duty %1.3f |", (Battery_Level*100), Battery_Value, LIN_Val, ANG_Val); + Duty_Mutex.unlock(); State_msg.data = buffer; Status_PUB.publish(&State_msg); } else { @@ -85,23 +92,52 @@ } } -void Control_CallBack(const std_msgs::Int32MultiArray& msg) +int Remap(float value, float start1, float stop1, float start2, float stop2) +{ + return (int)(start2 + (stop2 - start2) * ((value - start1) / (stop1 - start1))); +} + + + +void Control_CallBack(const std_msgs::Float32MultiArray& cmd_vel_msg) { /**************************************************************************/ //Extracting the only commands that will be used in the multiarray, and assuming that the int value attached to each point is 100 times //the intended PWM constant for that movement command (e.g. a value of 50 in msg.data[6] would give a PWM duty cycle of 0.5 for rotation): - float xlin = 0.01 * msg.data[1]; - float zrot = 0.01 * msg.data[6]; + float xlin, zang; + if((cmd_vel_msg.data[0] <= 1.0f) && (cmd_vel_msg.data[0] >= -1.0f)) + { + xlin = cmd_vel_msg.data[0]; + Duty_Mutex.lock(); + LIN_Val = xlin; + Duty_Mutex.unlock();; + } + if((cmd_vel_msg.data[1] <= 1.0f) && (cmd_vel_msg.data[1] >= -1.0f)) + { + zang = cmd_vel_msg.data[1]; + Duty_Mutex.lock(); + ANG_Val = zang; + Duty_Mutex.unlock(); + } + //assume rotation needs to be taken care of first, generally, then linear movement for this basic controller - if(zrot != 0) { //assume positive z is clockwise, negative is anticlockwise, A is left motor, B is right motor (viewed from bottom layer battery switch direction) - A.speed(-zrot); - B.speed(zrot); + //assume positive z is clockwise, negative is anticlockwise, A is left motor, B is right motor (viewed from bottom layer battery switch direction) + if(zang == 0.0f && xlin == 0.0f) { + A.stop(0.5f); + B.stop(0.5f); + wait(1); + A.coast(); + B.coast(); + } else if(zang != 0.0f && xlin !=0.0f) { + // Needs testing + A.speed((-xlin + zang)/2.0f); + B.speed((-xlin + -zang)/2.0f); } else if(xlin != 0) { - A.speed(xlin); - B.speed(xlin); - } else { - A.stop(1); - B.stop(1); + A.speed(-xlin); + B.speed(-xlin); + } else if(zang != 0) { + A.speed(zang); + B.speed(-zang); } /**************************************************************************/ }
--- a/ROS_Handler.hpp Thu Dec 19 00:13:38 2019 +0000 +++ b/ROS_Handler.hpp Sun Jan 05 15:42:22 2020 +0000 @@ -4,18 +4,13 @@ #include "rtos.h" #include "Pins.h" -#include <std_msgs/Empty.h> -#include <std_msgs/Int32.h> -#include <std_msgs/Int32MultiArray.h> #include <std_msgs/Float32MultiArray.h> -#include "std_msgs/String.h" -#include <geometry_msgs/Twist.h> -#include <geometry_msgs/Vector3.h> +#include <std_msgs/String.h> #ifndef Define_ONCE_ROShandler #define Define_ONCE_ROShandler void ROS_Handler(void); -void CallBack(const geometry_msgs::Twist& msg); +void Control_CallBack(const std_msgs::Float32MultiArray& cmd_vel_msg); #endif \ No newline at end of file
--- a/VL6180.cpp Thu Dec 19 00:13:38 2019 +0000 +++ b/VL6180.cpp Sun Jan 05 15:42:22 2020 +0000 @@ -1,59 +1,229 @@ #include "mbed.h" -#include "General.hpp" -#include "rtos.h" +//#include "General.hpp" +//#include "rtos.h" #include "Pins.h" #include "VL6180.hpp" #include <vector> -//#define DEBUG_TOF +#define DEBUG_TOF #ifdef DEBUG_TOF Serial debug_term(SERIAL_TX, SERIAL_RX); #endif - - -#define ROS_TOF +// +////#define ROS_TOF int Global_TOF_Ranges[num_VL6180]; bool TOF_Range_Flag; Mutex Global_TOF_Range_Mutex; +BusOut SHDN_Pins(PC_9, PC_11, PD_2, PG_3); + +I2C i2c(I2C_SDA, I2C_SCL); // Set up I²C on the STM32 NUCLEO-401RE + +/////////////////////////////////////////////////////////////////// +// Split 16-bit register address into two bytes and write +// the address + data via I²C +/////////////////////////////////////////////////////////////////// + +void WriteByte_Uninitialized(wchar_t reg, char data, char addr) { + char data_write[3]; + data_write[0] = (reg >> 8) & 0xFF;; + // MSB of register address + data_write[1] = reg & 0xFF; + // LSB of register address + data_write[2] = data & 0xFF; + i2c.write(addr, data_write, 3); +} + +/////////////////////////////////////////////////////////////////// +// Split 16-bit register address into two bytes and write +// required register address to VL6180 and read the data back +/////////////////////////////////////////////////////////////////// + +char ReadByte(wchar_t reg, char addr) { + char data_write[2]; + char data_read[1]; + + data_write[0] = (reg >> 8) & 0xFF; // MSB of register address + data_write[1] = reg & 0xFF; // LSB of register address + + i2c.write(addr, data_write, 2); + i2c.read(addr, data_read, 1); + return data_read[0]; +} + +/////////////////////////////////////////////////////////////////// +// load settings +/////////////////////////////////////////////////////////////////// + +int VL6180_Init(char addr) { + char reset; + + // check to see has it be Initialised already + reset = ReadByte(0x016, addr); + if (reset==1) + { + /////////////////////////////////////////////////////////////////// + // DEFAULT SETTINGS + /////////////////////////////////////////////////////////////////// + + // Mandatory : private registers + WriteByte_Uninitialized(0x0207, 0x01, addr); + WriteByte_Uninitialized(0x0208, 0x01, addr); + WriteByte_Uninitialized(0x0096, 0x00, addr); + WriteByte_Uninitialized(0x0097, 0xfd, addr); + WriteByte_Uninitialized(0x00e3, 0x01, addr); + WriteByte_Uninitialized(0x00e4, 0x03, addr); + WriteByte_Uninitialized(0x00e5, 0x02, addr); + WriteByte_Uninitialized(0x00e6, 0x01, addr); + WriteByte_Uninitialized(0x00e7, 0x03, addr); + WriteByte_Uninitialized(0x00f5, 0x02, addr); + WriteByte_Uninitialized(0x00d9, 0x05, addr); + WriteByte_Uninitialized(0x00db, 0xce, addr); + WriteByte_Uninitialized(0x00dc, 0x03, addr); + WriteByte_Uninitialized(0x00dd, 0xf8, addr); + WriteByte_Uninitialized(0x009f, 0x00, addr); + WriteByte_Uninitialized(0x00a3, 0x3c, addr); + WriteByte_Uninitialized(0x00b7, 0x00, addr); + WriteByte_Uninitialized(0x00bb, 0x3c, addr); + WriteByte_Uninitialized(0x00b2, 0x09, addr); + WriteByte_Uninitialized(0x00ca, 0x09, addr); + WriteByte_Uninitialized(0x0198, 0x01, addr); + WriteByte_Uninitialized(0x01b0, 0x17, addr); + WriteByte_Uninitialized(0x01ad, 0x00, addr); + WriteByte_Uninitialized(0x00ff, 0x05, addr); + WriteByte_Uninitialized(0x0100, 0x05, addr); + WriteByte_Uninitialized(0x0199, 0x05, addr); + WriteByte_Uninitialized(0x01a6, 0x1b, addr); + WriteByte_Uninitialized(0x01ac, 0x3e, addr); + WriteByte_Uninitialized(0x01a7, 0x1f, addr); + WriteByte_Uninitialized(0x0030, 0x00, addr); + WriteByte_Uninitialized(0x016, 0x00, addr); //change fresh out of set status to 0 + } + else { + return -1; + } + return 0; +} + +/////////////////////////////////////////////////////////////////// +// Start a range measurement in single shot mode +/////////////////////////////////////////////////////////////////// + +int VL6180_Start_Range(char addr) { + WriteByte_Uninitialized(0x018,0x03, addr); + return 0; +} + +/////////////////////////////////////////////////////////////////// +// poll for new sample ready ready +/////////////////////////////////////////////////////////////////// + +int VL6180_Poll_Range(char addr) { + char status; + char range_status; + + // check the status + status = ReadByte(0x04f,addr); + range_status = status & 0x07; + + // wait for new measurement ready status + while (range_status != 0x00) + { + status = ReadByte(0x04f,addr); + range_status = status & 0x07; + //wait_ms(50); // (can be removed) + } + + return 0; +} + + +/////////////////////////////////////////////////////////////////// +// Read range result (mm) +/////////////////////////////////////////////////////////////////// + +int VL6180_Read_Range(char addr) { + int range; + range=ReadByte(0x062, addr); + return range; +} + +/////////////////////////////////////////////////////////////////// +// clear interrupts +/////////////////////////////////////////////////////////////////// + +int VL6180_Clear_Interrupts(char addr) { + WriteByte_Uninitialized(0x015,0x07, addr); + return 0; +} + +/////////////////////////////////////////////////////////////////// +// Main Program loop +/////////////////////////////////////////////////////////////////// /*------------------------------------------------------------------------------ // Main Program loop ------------------------------------------------------------------------------*/ -int TOF_Thread() +void TOF_Handler() { - /* Pins are (PC_9, PC_11, PD_2, PG_3) */ - I2C i2c_bus(I2C_SDA, I2C_SCL); // Set up I²C on the STM32 NUCLEO-401RE + SHDN_Pins = 0; // 0x0 or 0b0000 + wait_ms(0.5); + + SHDN_Pins = 2; // 0x2 or 0b0010 - //Dynamically create VL6180 objects for the TOF sensors - PinName SHDN_Pins[num_VL6180] = SHDN_Pins_Cell; - char Addresses[num_VL6180] = Shifted_TOF_Addresses; - TOFsPtr * TOF = new TOFsPtr[num_VL6180]; - for (int idx = 0; idx < num_VL6180; ++idx) { - TOF[idx] = new VL6180(i2c_bus, SHDN_Pins[idx], Addresses[idx]); - } - - // Initialise all TOF sensors - Init_All_TOFs(TOF); + VL6180_Init(address1shift); + WriteByte_Uninitialized(0x212, address2, address1shift); + + SHDN_Pins = 6; // 0x6 or 0b0110 + VL6180_Init(address1shift); + WriteByte_Uninitialized(0x212, address3, address1shift); + + SHDN_Pins = 14; // 0xE or 0b1110 + VL6180_Init(address1shift); + WriteByte_Uninitialized(0x212, address4, address1shift); + + SHDN_Pins = 15; // 0xF or 0b1111 + VL6180_Init(address1shift); - //Create range variable to stored read TOF values - int Range[num_VL6180] = {}; - - //note that sensor 1 can be left at the default address, since other sensors - //are already allocated. Adding more I2C devices would require it to be - //reassigned to a different address, however. - while (1) { - for(int IDX = 0; IDX < num_VL6180; IDX++) { - TOF[IDX]->Start_Range(); - TOF[IDX]->Poll_Range(); - Range[IDX] = TOF[IDX]->Read_Range(); - TOF[IDX]->Clear_Interrupts(); - - wait_ms(10); - } + while (1) + { + Get_TOF_Reading(); + wait_ms(100); + } +} + +void Get_TOF_Reading() +{ + int Range[4]; + + // start range measurement + VL6180_Start_Range(address1shift); + // poll the VL6180 till new sample ready + VL6180_Poll_Range(address1shift); + Range[0] = VL6180_Read_Range(address1shift); + VL6180_Clear_Interrupts(address1shift); + wait_ms(10); + + // read range result + VL6180_Start_Range(address2shift); + VL6180_Poll_Range(address2shift); + Range[1] = VL6180_Read_Range(address2shift); + VL6180_Clear_Interrupts(address2shift); + wait_ms(10); + + VL6180_Start_Range(address3shift); + VL6180_Poll_Range(address3shift); + Range[2] = VL6180_Read_Range(address3shift); + VL6180_Clear_Interrupts(address3shift); + wait_ms(10); + + VL6180_Start_Range(address4shift); + VL6180_Poll_Range(address4shift); + Range[3] = VL6180_Read_Range(address4shift); + VL6180_Clear_Interrupts(address4shift); #ifdef ROS_TOF Global_TOF_Range_Mutex.lock(); - for(int IDX = 0; IDX < num_VL6180; IDX++) { + for(int IDX = 0; IDX < 4; IDX++) { Global_TOF_Ranges[IDX] = Range[IDX]; } if(TOF_Range_Flag == false) @@ -62,220 +232,4 @@ } Global_TOF_Range_Mutex.unlock(); #endif - -#ifdef DEBUG_TOF - debug_term.printf("|--------------------------------\n\r" - "|Ranges: \n\r" - "| %d\n\r",Range[0]); - - /* - "| %d\n\r" - "| %d\n\r" - "| %d\n\r",Range[0], Range[1], Range[2], Range[3]); - */ -#endif - - wait_ms(100); - - } } - - -/*------------------------------------------------------------------------------ - Function turns TOF sensor ON or OFF -------------------------------------------------------------------------------*/ -bool VL6180::TOF_PWR(bool State) -{ - this->shutdown = State; - return State; -} - -/*------------------------------------------------------------------------------ - Split 16-bit register address into two bytes and write - the address + data via I²C -------------------------------------------------------------------------------*/ -void VL6180::WriteByte(wchar_t reg, char data) -{ - char data_write[3]; - data_write[0] = (reg >> 8) & 0xFF;; - // MSB of register address - data_write[1] = reg & 0xFF; - // LSB of register address - data_write[2] = data & 0xFF; - this->i2c.write(addr, data_write, 3); -} -void VL6180::WriteByte_Uninitialized(wchar_t reg, char data, char Uninitialized_Address) { - char data_write[3]; - data_write[0] = (reg >> 8) & 0xFF;; - // MSB of register address - data_write[1] = reg & 0xFF; - // LSB of register address - data_write[2] = data & 0xFF; - this->i2c.write(Uninitialized_Address, data_write, 3); -} - -/*------------------------------------------------------------------------------ - Split 16-bit register address into two bytes and write - required register address to VL6180 and read the data back -------------------------------------------------------------------------------*/ -char VL6180::ReadByte(wchar_t reg) -{ - char data_write[2]; - char data_read[1]; - - data_write[0] = (reg >> 8) & 0xFF; // MSB of register address - data_write[1] = reg & 0xFF; // LSB of register address - - this->i2c.write(addr, data_write, 2); - this->i2c.read(addr, data_read, 1); - return data_read[0]; -} -char VL6180::ReadByte_Uninitialized(wchar_t reg, char Uninitialized_Address) { - char data_write[2]; - char data_read[1]; - - data_write[0] = (reg >> 8) & 0xFF; // MSB of register address - data_write[1] = reg & 0xFF; // LSB of register address - - this->i2c.write(Uninitialized_Address, data_write, 2); - this->i2c.read(Uninitialized_Address, data_read, 1); - return data_read[0]; -} - -/*------------------------------------------------------------------------------ - Start a range measurement in single shot mode -------------------------------------------------------------------------------*/ -int VL6180::Start_Range(void) -{ - this->WriteByte(0x018,0x03); - return 0; -} - -/*------------------------------------------------------------------------------ - poll for new sample ready ready -------------------------------------------------------------------------------*/ -int VL6180::Poll_Range(void) -{ - char status; - char range_status; - - // check the status - status = this->ReadByte(0x04f); - range_status = status & 0x07; - - // wait for new measurement ready status - while (range_status != 0x00) { - status = this->ReadByte(0x04f); - range_status = status & 0x07; - //wait_ms(50); // (can be removed) - } - - return 0; -} - - -/*------------------------------------------------------------------------------ - Read range result (mm) -------------------------------------------------------------------------------*/ -int VL6180::Read_Range(void) -{ - int range; - range = this->ReadByte(0x062); - return range; -} - -/*------------------------------------------------------------------------------ - clear interrupts -------------------------------------------------------------------------------*/ - -int VL6180::Clear_Interrupts(void) -{ - this->WriteByte(0x015,0x07); - return 0; -} - -/*------------------------------------------------------------------------------ - load settings -------------------------------------------------------------------------------*/ -bool VL6180::Init(void) -{ - // check to see has if the TOF sensor has been Initialised already - char reset = this->ReadByte_Uninitialized(0x016, 0x82); - if (reset==1) - { - // Mandatory : private registers - this->WriteByte_Uninitialized(0x0207, 0x01, 0x82); - this->WriteByte_Uninitialized(0x0208, 0x01, 0x82); - this->WriteByte_Uninitialized(0x0096, 0x00, 0x82); - this->WriteByte_Uninitialized(0x0097, 0xfd, 0x82); - this->WriteByte_Uninitialized(0x00e3, 0x01, 0x82); - this->WriteByte_Uninitialized(0x00e4, 0x03, 0x82); - this->WriteByte_Uninitialized(0x00e5, 0x02, 0x82); - this->WriteByte_Uninitialized(0x00e6, 0x01, 0x82); - this->WriteByte_Uninitialized(0x00e7, 0x03, 0x82); - this->WriteByte_Uninitialized(0x00f5, 0x02, 0x82); - this->WriteByte_Uninitialized(0x00d9, 0x05, 0x82); - this->WriteByte_Uninitialized(0x00db, 0xce, 0x82); - this->WriteByte_Uninitialized(0x00dc, 0x03, 0x82); - this->WriteByte_Uninitialized(0x00dd, 0xf8, 0x82); - this->WriteByte_Uninitialized(0x009f, 0x00, 0x82); - this->WriteByte_Uninitialized(0x00a3, 0x3c, 0x82); - this->WriteByte_Uninitialized(0x00b7, 0x00, 0x82); - this->WriteByte_Uninitialized(0x00bb, 0x3c, 0x82); - this->WriteByte_Uninitialized(0x00b2, 0x09, 0x82); - this->WriteByte_Uninitialized(0x00ca, 0x09, 0x82); - this->WriteByte_Uninitialized(0x0198, 0x01, 0x82); - this->WriteByte_Uninitialized(0x01b0, 0x17, 0x82); - this->WriteByte_Uninitialized(0x01ad, 0x00, 0x82); - this->WriteByte_Uninitialized(0x00ff, 0x05, 0x82); - this->WriteByte_Uninitialized(0x0100, 0x05, 0x82); - this->WriteByte_Uninitialized(0x0199, 0x05, 0x82); - this->WriteByte_Uninitialized(0x01a6, 0x1b, 0x82); - this->WriteByte_Uninitialized(0x01ac, 0x3e, 0x82); - this->WriteByte_Uninitialized(0x01a7, 0x1f, 0x82); - this->WriteByte_Uninitialized(0x0030, 0x00, 0x82); - this->WriteByte_Uninitialized(0x016, 0x00, 0x82); //change fresh out of set status to 0 - } else { - return true; - } - return false; -} - - -/*------------------------------------------------------------------------------ - Function to Initialise all TOF sensors -------------------------------------------------------------------------------*/ -bool Init_All_TOFs(TOFsPtr *tof) -{ - bool Err_Flag = false; - - char Non_Shifted_Addresses[num_VL6180] = TOF_Addresses; - - for(int IDX = 0; IDX < num_VL6180; IDX++) - { - tof[IDX]->TOF_PWR(false); - } - - wait_ms(0.5); - - if(num_VL6180 > 1) - { - for(int IDX = 1; IDX < num_VL6180; IDX++) - { - tof[IDX]->TOF_PWR(true); - - if(tof[IDX]->Init() == true) - { - Err_Flag = true; - } - tof[IDX]->WriteByte(0x212, Non_Shifted_Addresses[IDX]); - - } - } - - tof[0]->TOF_PWR(true); - tof[0]->Init(); - tof[0]->WriteByte(0x212, Non_Shifted_Addresses[0]); - - return Err_Flag; -}
--- a/VL6180.hpp Thu Dec 19 00:13:38 2019 +0000 +++ b/VL6180.hpp Sun Jan 05 15:42:22 2020 +0000 @@ -1,6 +1,6 @@ #include "mbed.h" -#include "General.hpp" -#include "rtos.h" +//#include "General.hpp" +//#include "rtos.h" #include "Pins.h" #ifndef Define_ONCE_VL6180 @@ -11,39 +11,30 @@ #define TOF_Addresses {0x29} //, 0x2A, 0x2B, 0x2C} #define Shifted_TOF_Addresses {0x82} //, 0x84, 0x86, 0x88} -// Thread that runs TOF sensors -int TOF_Thread(); + +//Addresses +#define address1 (0x29) +#define address2 (0x2A) +#define address3 (0x2B) +#define address4 (0x2C) -// Class Terminal expects tx and rx pins and is used for controlling a serialy conected terminal -class VL6180 -{ -public: - VL6180(I2C& comm_bus, PinName SHDWN, char Address) : shutdown(SHDWN), i2c(comm_bus){addr = Address;}; +//Shifted addresses, so the R/W command can be added +#define address1shift (address1<<1) +#define address2shift (address2<<1) +#define address3shift (address3<<1) +#define address4shift (address4<<1) + - bool Init(void); - bool TOF_PWR(bool State); - void WriteByte(wchar_t reg, char data); - void WriteByte_Uninitialized(wchar_t reg, char data, char Uninitialized_Address); - int Start_Range(void); - int Poll_Range(void); - int Read_Range(void); - int Clear_Interrupts(void); - -private: - // I2C Bus TOF is connected to - I2C& i2c; - // Private functions - char ReadByte(wchar_t reg); - char ReadByte_Uninitialized(wchar_t reg, char Uninitialized_Address); - void WriteByte_Uninitialized(wchar_t reg, char data); - // Private Variables - char addr; - // IOs - DigitalOut shutdown; -}; +// Thread that runs TOF sensors +void TOF_Handler(); -// TypeDef for pointer to the TOF class (needed for dynamic creation of objects) -typedef VL6180* TOFsPtr; -// Function to Initialise all TOF sensors -bool Init_All_TOFs(TOFsPtr *tof); +int VL6180_Init(char addr); +void WriteByte_Uninitialized(wchar_t reg, char data, char addr); +char ReadByte(wchar_t reg, char addr); +int VL6180_Start_Range(char addr); +int VL6180_Poll_Range(char addr); +int VL6180_Read_Range(char addr); +int VL6180_Clear_Interrupts(char addr); +void Get_TOF_Reading(void); + #endif
--- a/main.cpp Thu Dec 19 00:13:38 2019 +0000 +++ b/main.cpp Sun Jan 05 15:42:22 2020 +0000 @@ -17,14 +17,15 @@ #include <motordriver.h> #include "math.h" #include "Motors.hpp" -//#include "VL6180.hpp" +#include "VL6180.hpp" DigitalOut debug_LED(LED1); // Threads Thread ROS_Thread(osPriorityRealtime); // Create THREAD with highest priority for ROS +Thread TOF_Thread(osPriorityNormal); //Thread Debug_Thread(osPriorityNormal); -//Thread Movement_Thread(osPriorityNormal); +//Thread Motor_Thread(osPriorityNormal); // Thread ID for the Main function (CMSIS API) osThreadId tidMain; @@ -32,20 +33,20 @@ // main starts all threads int main(void) { - //TOF_Thread(); // Main thread ID tidMain = Thread::gettid(); // Start each thread ROS_Thread.start(ROS_Handler); + TOF_Thread.start(TOF_Handler); + //Motor_Thread.start(Motor_Handler); //Debug_Thread.start(TerminalThread); - //Thread2.start(main3); // Creates an object to monitor and handle changes in battery level using an RGB LED as an OutPut - Battery_Monitor VBatt_Monitor(ADC_VBAT, 0.5f); + Battery_Monitor VBatt_Monitor(ADC_VBAT, 1.0f); /**************************************************************************/ - initialize(); //initialize(); only needs to be called once, at the start of the program. + //initialize(); //initialize(); only needs to be called once, at the start of the program. //the encoder interrupts should be able to handle themselves from there, and do not require //resetting. /**************************************************************************/ @@ -54,21 +55,8 @@ { //Flag_Error(warning, "Flashing LED\n\r"); debug_LED = 1; - wait(1); + wait_ms(500); debug_LED = 0; - wait(1); - - - /**********************************************************************/ - //careful, putting this in a while loop will mean the bot moves in an infinite square! - /**********************************************************************/ - //tempMove(40); - //tempRotate(-90); - //tempMove(40); - //tempRotate(-90); - //tempMove(40); - //tempRotate(-90); - //tempMove(40); - //tempRotate(-90); + wait_ms(500); } }