diff --git a/qt_pi/qt_pi/CMakeLists.txt b/qt_pi/qt_pi/CMakeLists.txt new file mode 100644 index 0000000..412801a --- /dev/null +++ b/qt_pi/qt_pi/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 2.8.3) +project(qt_pi) + +find_package(catkin REQUIRED COMPONENTS + kinect2_bridge + qt_pi_serial + robot_localization +) + +catkin_package() + diff --git a/qt_pi/qt_pi/launch/arduino.launch b/qt_pi/qt_pi/launch/arduino.launch deleted file mode 100644 index 77277fd..0000000 --- a/qt_pi/qt_pi/launch/arduino.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/qt_pi/qt_pi/launch/localization.launch b/qt_pi/qt_pi/launch/localization.launch new file mode 100644 index 0000000..d8533a9 --- /dev/null +++ b/qt_pi/qt_pi/launch/localization.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + [false, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false ] + + [false, false, false, + false, false, false, + false, false, false, + false, false, true, + true, false, false ] + + diff --git a/qt_pi/qt_pi/launch/qt_pi.launch b/qt_pi/qt_pi/launch/qt_pi.launch index c176b7a..9618baa 100644 --- a/qt_pi/qt_pi/launch/qt_pi.launch +++ b/qt_pi/qt_pi/launch/qt_pi.launch @@ -1,10 +1,5 @@ - - - - - - - + + diff --git a/qt_pi/qt_pi/package.xml b/qt_pi/qt_pi/package.xml new file mode 100644 index 0000000..f448fe6 --- /dev/null +++ b/qt_pi/qt_pi/package.xml @@ -0,0 +1,25 @@ + + + qt_pi + 0.0.0 + The qt_pi package + + Rishabh Kundu + + MIT + + catkin + kinect2_bridge + qt_pi_serial + robot_localization + kinect2_bridge + qt_pi_serial + robot_localization + kinect2_bridge + qt_pi_serial + robot_localization + + + + + diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/batt.cpp b/qt_pi/qt_pi_embedded/src/embedded_firmware/batt.cpp index d2c8ae6..2bd0fee 100644 --- a/qt_pi/qt_pi_embedded/src/embedded_firmware/batt.cpp +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/batt.cpp @@ -1,6 +1,6 @@ #include "qt_pi.h" -float batt_lvl; +uint16_t batt_lvl; void initBatt(){ @@ -13,14 +13,15 @@ void initBatt(){ void updateBattLevel(){ - batt_lvl = ( analogRead(BATT_PIN) * 12.22 / 800.0 ) * 0.001 + batt_lvl * 0.999; - + for(int i = 0; i < 10; i++) + batt_lvl += analogRead(BATT_PIN); + + batt_lvl /= 10; } void getBattLvl(uint8_t *buf){ - - uint16_t tmp = (uint16_t)(batt_lvl * 100); - - memcpy(buf, &tmp, 2); + + updateBattLevel(); + memcpy(buf, &batt_lvl, 2); } diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/batt.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/batt.h index 9c2bac4..02382b3 100644 --- a/qt_pi/qt_pi_embedded/src/embedded_firmware/batt.h +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/batt.h @@ -3,7 +3,7 @@ #define BATT_PIN A0 -const int BATT_INTERVAL = 1000; +const int BATT_INTERVAL = 30000; extern void initBatt(); extern void updateBattLevel(); diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/comm.cpp b/qt_pi/qt_pi_embedded/src/embedded_firmware/comm.cpp index f290970..71d900b 100644 --- a/qt_pi/qt_pi_embedded/src/embedded_firmware/comm.cpp +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/comm.cpp @@ -1,13 +1,5 @@ #include "qt_pi.h" -/* -#define WAITING_FOR_NEW_COM 0 -#define FIRST_BYTE_VERIFIED 1 -#define SECOND_BYTE_VERIFIED 2 -#define LENGTH_INPUT 3 -#define READING_PAYLOAD 4 -#define READING_COMPLETE 5 -#define COM_RECEIVED 6 -*/ + enum COMM_STATE { WAITING_FOR_NEW_COM, @@ -53,7 +45,7 @@ sendCommand (int16_t len) memcpy (ser_write_buf + start_magic_len, &len, len_len); ser_write_buf[preamble_len + len] = magic_end; - + return (Serial.write (ser_write_buf, len + net_extra_len)); } @@ -137,21 +129,7 @@ runInpCommand () } move_cmd_timeout_reset (); - - if (com_recv[DATASTRB] == 0 && com_recv[DATASTRB + 1] == 0) - { - - setMotorSpeeds (0, 0); - - resetPID (); - - setMoving (false); - } - else - { - setMoving (true); - } - + setTicksPerFrame (com_recv + DATASTRB); ser_write_buf[DATASTRBP] = 1; @@ -213,7 +191,7 @@ runInpCommand () getBattLvl (ser_write_buf + DATASTRBP); - data_len = 2; + data_len = 4; break; @@ -283,6 +261,7 @@ uint8_t temp_buf[64]; void serCommTimeout () { + resetComm(); Serial.readBytes (temp_buf, 64); } @@ -293,7 +272,7 @@ proc_serial () if (!Serial.available ()) return; - move_cmd_timeout_reset (); + ser_comm_timeout_reset (); switch (curState) { diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/comm.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/comm.h index dbf5bbd..a4b56a3 100644 --- a/qt_pi/qt_pi_embedded/src/embedded_firmware/comm.h +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/comm.h @@ -2,11 +2,13 @@ #define COMM_H const unsigned long BAUDRATE = 115200; +#define COMM_RESET_INTERVAL 1000 extern int init_comm(); extern void proc_serial(); extern int sendData(int, uint8_t *buf); extern void serCommTimeout(); extern void resetComm(); +extern void comm_check(); #endif diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/commands.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/commands.h index 2a520a4..830378e 100644 --- a/qt_pi/qt_pi_embedded/src/embedded_firmware/commands.h +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/commands.h @@ -19,7 +19,7 @@ #define GET_BAUDRATE_DATA_LEN 0x01 #define GET_ENCODERS_DATA_LEN 0x01 -#define SET_SPEEDS_DATA_LEN 0x03 +#define SET_SPEEDS_DATA_LEN 0x08 #define RESET_ENCODERS_DATA_LEN 0x01 #define SET_PID_DATA_LEN 0x04 #define GET_PID_P_DATA_LEN 0x01 diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/diff_controller.cpp b/qt_pi/qt_pi_embedded/src/embedded_firmware/diff_controller.cpp index 34bd27a..5419e62 100644 --- a/qt_pi/qt_pi_embedded/src/embedded_firmware/diff_controller.cpp +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/diff_controller.cpp @@ -2,7 +2,7 @@ typedef struct { - double TargetTicksPerFrame; + float TargetTicksPerFrame; long Encoder; long PrevEnc; @@ -24,10 +24,10 @@ SetPointInfo; SetPointInfo leftPID, rightPID; -unsigned char moving = 0; +bool isMoving = false; void resetPID(){ - + leftPID.TargetTicksPerFrame = 0.0; leftPID.Encoder = readEncoder(LEFT_WHEEL); leftPID.PrevEnc = leftPID.Encoder; @@ -85,10 +85,7 @@ void doPID(SetPointInfo * p) { void updatePID() { - leftPID.Encoder = readEncoder(LEFT_WHEEL); - rightPID.Encoder = readEncoder(RIGHT_WHEEL); - - if (!moving){ + if (!isMoving){ if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID(); @@ -98,15 +95,27 @@ void updatePID() { if(leftPID.TargetTicksPerFrame != 0) { + leftPID.Encoder = readEncoder(LEFT_WHEEL); doPID(&leftPID); } if(rightPID.TargetTicksPerFrame != 0) { + rightPID.Encoder = readEncoder(RIGHT_WHEEL); doPID(&rightPID); } - setMotorSpeeds(leftPID.output, rightPID.output); + if((abs(leftPID.output) < 70) && (abs(rightPID.output) < 70)) + setMotorSpeeds(0, 0); + + else if(abs(leftPID.output) < 70) + setMotorSpeeds(0, rightPID.output); + + else if(abs(rightPID.output) < 70) + setMotorSpeeds(leftPID.output, 0); + + else + setMotorSpeeds(leftPID.output, rightPID.output); } void setPID_L(uint8_t *buf){ @@ -126,16 +135,19 @@ void setPID_R(uint8_t *buf){ rightPID.Ko = buf[3]; } -void moveTimeout(){ - - setMotorSpeeds(0, 0); - moving = 0; - resetPID(); - +void moveTimeout(){ + setMoving(false); } void setMoving(bool t){ - moving = t; + + if((!t) && (isMoving)) + { + setMotorSpeeds(0, 0); + resetPID(); + } + + isMoving = t; } void getPID(uint8_t *buf){ @@ -154,7 +166,16 @@ void getPID(uint8_t *buf){ void setTicksPerFrame(uint8_t *buf){ - leftPID.TargetTicksPerFrame = buf[0]; - rightPID.TargetTicksPerFrame = buf[1]; + int32_t l, r; + + memcpy(&l, buf, 4); + memcpy(&r, buf + 4, 4); + leftPID.TargetTicksPerFrame = l / 1000.0; + rightPID.TargetTicksPerFrame = r / 1000.0; + + if(l | r) + setMoving(true); + else + setMoving(false); } diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/embedded_firmware.ino b/qt_pi/qt_pi_embedded/src/embedded_firmware/embedded_firmware.ino index 5ab24d0..5617292 100644 --- a/qt_pi/qt_pi_embedded/src/embedded_firmware/embedded_firmware.ino +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/embedded_firmware.ino @@ -23,30 +23,36 @@ void setup() { initMotorController(); + initBatt(); + + resetPID(); + batt_mon = new Task(BATT_INTERVAL, TASK_FOREVER, &updateBattLevel, &ts, true, - &initBatt, NULL); + NULL, NULL); diff_control = new Task(PID_INTERVAL, TASK_FOREVER, &updatePID, &ts, true, - &resetPID, NULL); + NULL, NULL); move_cmd_timeout = - new Task(AUTO_STOP_INTERVAL, 1, &moveTimeout, &ts, true, NULL, NULL); + new Task(AUTO_STOP_INTERVAL, 1, &moveTimeout, &ts, false, NULL, NULL); odom_out = new Task(ODOM_INTERVAL, TASK_FOREVER, &sendOdom, &ts, false, NULL, NULL); imu_out = new Task(IMU_INTERVAL, TASK_FOREVER, &sendIMU, &ts, false, &initIMU, NULL); ser_comm_timeout = - new Task(AUTO_STOP_INTERVAL, 1, &serCommTimeout, &ts, true, NULL, NULL); + new Task(COMM_RESET_INTERVAL, 1, &serCommTimeout, &ts, false, NULL, NULL); + } void loop() { - ts.execute(); - proc_serial(); - +} + +void ser_comm_timeout_reset(){ + ser_comm_timeout->restartDelayed(COMM_RESET_INTERVAL); } void move_cmd_timeout_reset() { - // move_cmd_timeout->resetTimeout(); + move_cmd_timeout->restartDelayed(AUTO_STOP_INTERVAL); } void enable_odom(bool a) { diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/encoder_driver.cpp b/qt_pi/qt_pi_embedded/src/embedded_firmware/encoder_driver.cpp index 614e9d6..13700a3 100644 --- a/qt_pi/qt_pi_embedded/src/embedded_firmware/encoder_driver.cpp +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/encoder_driver.cpp @@ -59,35 +59,23 @@ long readEncoder(int i) { } - void resetEncoder(int i) { - - if (i == LEFT_WHEEL){ - left_enc_pos=0L; - return; - } - else - { - right_enc_pos=0L; - return; - } - } - void resetEncoders() { - resetEncoder(LEFT_WHEEL); - resetEncoder(RIGHT_WHEEL); - + left_enc_pos=0L; + right_enc_pos=0L; + resetPID(); } +uint8_t bufodom[8 + com_preamble_len]; + void sendOdom(){ - static uint8_t buf[8 + com_preamble_len]; - - buf[COMB] = ODOM_DATA; - buf[CIDB] = 0xFF; + bufodom[COMB] = ODOM_DATA; + bufodom[CIDB] = 0xFF; - memcpy(buf + DATASTRB, &left_enc_pos, 4); - memcpy(buf + DATASTRB + 4, &right_enc_pos, 4); + memcpy(bufodom + DATASTRB, &left_enc_pos, 4); + memcpy(bufodom + DATASTRB + 4, &right_enc_pos, 4); + + sendData(8 + com_preamble_len, bufodom); - sendData(buf, 8 + com_preamble_len); } diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/imu.cpp b/qt_pi/qt_pi_embedded/src/embedded_firmware/imu.cpp index 006bd2d..3453c8a 100644 --- a/qt_pi/qt_pi_embedded/src/embedded_firmware/imu.cpp +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/imu.cpp @@ -9,35 +9,30 @@ void initIMU(){ I2Cdev::writeBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO); I2Cdev::writeBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250); - I2Cdev::writeBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2); - + I2Cdev::writeBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, false); I2Cdev::readBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, &buf); - // use the code below to change accel/gyro offset values - /* - Serial.println("Updating internal sensor offsets..."); - // -76 -2359 1688 0 0 0 - Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76 - Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359 - Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688 - accelgyro.setXGyroOffset(220); - accelgyro.setYGyroOffset(76); - accelgyro.setZGyroOffset(-85); -*/ + I2Cdev::writeWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_XA_OFFS_H, 620); + I2Cdev::writeWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_YA_OFFS_H, -2300); + I2Cdev::writeWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ZA_OFFS_H, 1500); + I2Cdev::writeWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_XG_OFFS_USRH, 29); + I2Cdev::writeWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_YG_OFFS_USRH, 0); + I2Cdev::writeWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ZG_OFFS_USRH, 0); + } -void sendIMU() { +uint8_t bufimu[14 + com_preamble_len]; - static uint8_t buf[14 + com_preamble_len]; +void sendIMU() { - I2Cdev::readBytes(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, 14, buf + DATASTRB); + I2Cdev::readBytes(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, 14, bufimu + DATASTRB); - buf[COMB] = IMU_DATA; - buf[CIDB] = BID; - - sendData(14 + com_preamble_len, buf); + bufimu[COMB] = IMU_DATA; + bufimu[CIDB] = BID; + + sendData(14 + com_preamble_len, bufimu); } diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/imu.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/imu.h index 826a2a2..e791c8f 100644 --- a/qt_pi/qt_pi_embedded/src/embedded_firmware/imu.h +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/imu.h @@ -25,6 +25,14 @@ #define MPU6050_RA_WHO_AM_I 0x75 #define MPU6050_RA_ACCEL_XOUT_H 0x3B +#define MPU6050_RA_XA_OFFS_H 0x06 +#define MPU6050_RA_YA_OFFS_H 0x08 +#define MPU6050_RA_ZA_OFFS_H 0x0A + +#define MPU6050_RA_XG_OFFS_USRH 0x13 +#define MPU6050_RA_YG_OFFS_USRH 0x15 +#define MPU6050_RA_ZG_OFFS_USRH 0x17 + #define IMU_RATE 100 const int IMU_INTERVAL = 1000 / IMU_RATE; diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/motor_driver.cpp b/qt_pi/qt_pi_embedded/src/embedded_firmware/motor_driver.cpp index 34f5f5a..1ae2f80 100644 --- a/qt_pi/qt_pi_embedded/src/embedded_firmware/motor_driver.cpp +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/motor_driver.cpp @@ -24,8 +24,16 @@ void setMotorSpeed(int i, int spd) { if(spd == 0) { - digitalWrite(LEFT_MOTOR_FORWARD, 1); - digitalWrite(LEFT_MOTOR_BACKWARD, 1); + switch(i){ + case LEFT_WHEEL: + digitalWrite(LEFT_MOTOR_FORWARD, 1); + digitalWrite(LEFT_MOTOR_BACKWARD, 1); + break; + case RIGHT_WHEEL: + digitalWrite(RIGHT_MOTOR_FORWARD, 1); + digitalWrite(RIGHT_MOTOR_BACKWARD, 1); + break; + } } else diff --git a/qt_pi/qt_pi_embedded/src/embedded_firmware/qt_pi.h b/qt_pi/qt_pi_embedded/src/embedded_firmware/qt_pi.h index 6c82459..62d44f0 100644 --- a/qt_pi/qt_pi_embedded/src/embedded_firmware/qt_pi.h +++ b/qt_pi/qt_pi_embedded/src/embedded_firmware/qt_pi.h @@ -14,6 +14,7 @@ #include "motor_driver.h" extern void move_cmd_timeout_reset(); +extern void ser_comm_timeout_reset(); extern void enable_imu(bool); extern void enable_odom(bool); diff --git a/qt_pi/qt_pi_msgs/msg/Encoder.msg b/qt_pi/qt_pi_msgs/msg/Encoder.msg index 121cce9..8c9cf48 100644 --- a/qt_pi/qt_pi_msgs/msg/Encoder.msg +++ b/qt_pi/qt_pi_msgs/msg/Encoder.msg @@ -1,2 +1,2 @@ -int64 LEFT -int64 RIGHT +int32 LEFT +int32 RIGHT diff --git a/qt_pi/qt_pi_msgs/msg/PID.msg b/qt_pi/qt_pi_msgs/msg/PID.msg index 792f971..d81a81e 100644 --- a/qt_pi/qt_pi_msgs/msg/PID.msg +++ b/qt_pi/qt_pi_msgs/msg/PID.msg @@ -1,8 +1,4 @@ -uint8 lkp -uint8 lkd -uint8 lki -uint8 lko -uint8 rkp -uint8 rkd -uint8 rki -uint8 rko +uint8 kp +uint8 kd +uint8 ki +uint8 ko diff --git a/qt_pi/qt_pi_msgs/srv/GetPIDData.srv b/qt_pi/qt_pi_msgs/srv/GetPIDData.srv index 3f4899f..fb1d42b 100644 --- a/qt_pi/qt_pi_msgs/srv/GetPIDData.srv +++ b/qt_pi/qt_pi_msgs/srv/GetPIDData.srv @@ -1,2 +1,4 @@ --- -qt_pi_msgs/PID data +qt_pi_msgs/PID LEFT +qt_pi_msgs/PID RIGHT + diff --git a/qt_pi/qt_pi_msgs/srv/SetPIDData.srv b/qt_pi/qt_pi_msgs/srv/SetPIDData.srv index 0807fe1..a8e84df 100644 --- a/qt_pi/qt_pi_msgs/srv/SetPIDData.srv +++ b/qt_pi/qt_pi_msgs/srv/SetPIDData.srv @@ -1,2 +1,3 @@ +int16 motor qt_pi_msgs/PID data --- diff --git a/qt_pi/qt_pi_serial/CMakeLists.txt b/qt_pi/qt_pi_serial/CMakeLists.txt index b3dbd15..c07802c 100644 --- a/qt_pi/qt_pi_serial/CMakeLists.txt +++ b/qt_pi/qt_pi_serial/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs nav_msgs std_srvs + tf ) catkin_package() @@ -30,3 +31,5 @@ target_link_libraries(serial_node serial_comm) add_executable(serial_comm_test tests/comm_test.cpp) target_link_libraries(serial_comm_test serial_comm) +add_executable(imu_test tests/imu_data_test.cpp) +target_link_libraries(imu_test serial_comm) diff --git a/qt_pi/qt_pi/config/qt_pi_arduino_params.yaml b/qt_pi/qt_pi_serial/config/qt_pi_arduino_params.yaml similarity index 100% rename from qt_pi/qt_pi/config/qt_pi_arduino_params.yaml rename to qt_pi/qt_pi_serial/config/qt_pi_arduino_params.yaml diff --git a/qt_pi/qt_pi_serial/include/qt_pi_serial/commands.h b/qt_pi/qt_pi_serial/include/qt_pi_serial/commands.h index 2a520a4..830378e 100644 --- a/qt_pi/qt_pi_serial/include/qt_pi_serial/commands.h +++ b/qt_pi/qt_pi_serial/include/qt_pi_serial/commands.h @@ -19,7 +19,7 @@ #define GET_BAUDRATE_DATA_LEN 0x01 #define GET_ENCODERS_DATA_LEN 0x01 -#define SET_SPEEDS_DATA_LEN 0x03 +#define SET_SPEEDS_DATA_LEN 0x08 #define RESET_ENCODERS_DATA_LEN 0x01 #define SET_PID_DATA_LEN 0x04 #define GET_PID_P_DATA_LEN 0x01 diff --git a/qt_pi/qt_pi_serial/include/qt_pi_serial/qt_pi_params.h b/qt_pi/qt_pi_serial/include/qt_pi_serial/qt_pi_params.h new file mode 100644 index 0000000..b2622e7 --- /dev/null +++ b/qt_pi/qt_pi_serial/include/qt_pi_serial/qt_pi_params.h @@ -0,0 +1,12 @@ +#ifndef QT_PI_PARAMS_H +#define QT_PI_PARAMS_H + +const float v_alpha = 0.25; +const double wheel_dia = 0.0703464848466; +const float wheel_sep = 0.205; +const int counts_per_rot = 3; +const float gear_ratio = 80; +const float height = 0.05; +const int ODOM_RATE = 33; + +#endif diff --git a/qt_pi/qt_pi_serial/include/qt_pi_serial/ros_node.h b/qt_pi/qt_pi_serial/include/qt_pi_serial/ros_node.h index 319cfe0..450a75a 100644 --- a/qt_pi/qt_pi_serial/include/qt_pi_serial/ros_node.h +++ b/qt_pi/qt_pi_serial/include/qt_pi_serial/ros_node.h @@ -6,41 +6,42 @@ #include #include #include +#include +#include +#include +#include +#include +#include #include "qt_pi_msgs/GetBatteryInfo.h" #include "qt_pi_msgs/GetBaudRate.h" #include "qt_pi_msgs/GetEncoderData.h" #include "qt_pi_msgs/GetPIDData.h" #include "qt_pi_msgs/SetPIDData.h" +#include "qt_pi_params.h" #include "serial_driver.h" -#include "std_srvs/SetBool.h" -#include "std_srvs/Trigger.h" ros::Publisher imu_pub; ros::Publisher odom_pub; -ros::ServiceServer get_encoder; -ros::ServiceServer get_batt_state; -ros::ServiceServer get_pid_val; -ros::ServiceServer get_baudrate; -ros::ServiceServer set_pid_val; -ros::ServiceServer en_imu; -ros::ServiceServer en_odom; -ros::ServiceServer reset_odom; -bool GET_ENC_SRV_CB(qt_pi_msgs::GetEncoderData::Request &req, - qt_pi_msgs::GetEncoderData::Response &res); -bool GET_BATT_SRV_CB(qt_pi_msgs::GetBatteryInfo::Request &req, - qt_pi_msgs::GetBatteryInfo::Response &res); -bool GET_PID_SRV_CB(qt_pi_msgs::GetPIDData::Request &req, - qt_pi_msgs::GetPIDData::Response &res); -bool GET_BAUD_SRV_CB(qt_pi_msgs::GetBaudRate::Request &req, - qt_pi_msgs::GetBaudRate::Response &res); -bool SET_PID_SRV_CB(qt_pi_msgs::SetPIDData::Request &req, - qt_pi_msgs::SetPIDData::Response &res); -bool EN_IMU_SRV_CB(std_srvs::SetBool::Request &req, - std_srvs::SetBool::Response &res); -bool EN_ODOM_SRV_CB(std_srvs::SetBool::Request &req, - std_srvs::SetBool::Response &res); -bool RESET_ODOM_SRV_CB(std_srvs::Trigger::Request &req, - std_srvs::Trigger::Response &res); +ros::ServiceServer get_batt_state; +ros::ServiceServer get_baudrate; + +ros::ServiceServer get_pid_val; +ros::ServiceServer set_pid_val; + +ros::ServiceServer get_encoder; +ros::ServiceServer reset_odom; +ros::ServiceServer en_odom; + +ros::ServiceServer en_imu; +ros::Subscriber imu_tare; + +ros::Subscriber move_cmd; + +int16_t imu_offset[6]; //{-18, 117, -63, 29, -21, -17}; + +void init_node(ros::NodeHandle n); +void shutdown_node(); + #endif diff --git a/qt_pi/qt_pi_serial/include/qt_pi_serial/serial_driver.h b/qt_pi/qt_pi_serial/include/qt_pi_serial/serial_driver.h index edc9c16..18fc1f1 100644 --- a/qt_pi/qt_pi_serial/include/qt_pi_serial/serial_driver.h +++ b/qt_pi/qt_pi_serial/include/qt_pi_serial/serial_driver.h @@ -24,4 +24,5 @@ extern int sendCom(); extern void IMU_DATA_PUBLISH(unsigned char *); extern void ODOM_DATA_PUBLISH(unsigned char *); +extern void set_CB(void (*)(unsigned char *)); #endif diff --git a/qt_pi/qt_pi_serial/launch/serial.launch b/qt_pi/qt_pi_serial/launch/serial.launch new file mode 100644 index 0000000..f597914 --- /dev/null +++ b/qt_pi/qt_pi_serial/launch/serial.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/qt_pi/qt_pi_serial/package.xml b/qt_pi/qt_pi_serial/package.xml index c9bb6a4..b778840 100644 --- a/qt_pi/qt_pi_serial/package.xml +++ b/qt_pi/qt_pi_serial/package.xml @@ -14,6 +14,7 @@ sensor_msgs std_msgs std_srvs + tf roscpp qt_pi_msgs @@ -22,4 +23,5 @@ sensor_msgs std_msgs std_srvs + tf diff --git a/qt_pi/qt_pi_serial/src/main.cpp b/qt_pi/qt_pi_serial/src/main.cpp index 15bf94d..ec7a0ad 100644 --- a/qt_pi/qt_pi_serial/src/main.cpp +++ b/qt_pi/qt_pi_serial/src/main.cpp @@ -3,9 +3,14 @@ void sighandler(int signum) { printf("Caught signal %d, Exiting after thread completion\n", signum); + + shutdown_node(); close_comm(); + printf("COMM CLOSED\n"); + ros::shutdown(); + printf("shutting down\n"); } @@ -14,32 +19,8 @@ int main(int argc, char **argv) { ros::init(argc, argv, "serial_driver"); ros::NodeHandle n; - ROS_INFO("REACHED1\n"); - if (init_comm("/dev/ttyUSB0") < 0) { - ROS_FATAL("Unable to open port"); - exit(0); - } - imu_pub = n.advertise("imu", 1); - ROS_INFO("REACHED2\n"); - odom_pub = n.advertise("odom", 1); - ROS_INFO("REACHED3\n"); - get_encoder = n.advertiseService("get_encoder_state", GET_ENC_SRV_CB); - ROS_INFO("REACHED4\n"); - get_batt_state = n.advertiseService("get_battery_state", GET_BATT_SRV_CB); - ROS_INFO("REACHED5\n"); - get_pid_val = n.advertiseService("get_pid_values", GET_PID_SRV_CB); - ROS_INFO("REACHED6\n"); - get_baudrate = n.advertiseService("get_baudrate", GET_BAUD_SRV_CB); - ROS_INFO("REACHED7\n"); - set_pid_val = n.advertiseService("set_pid_values", SET_PID_SRV_CB); - ROS_INFO("REACHED8\n"); - en_imu = n.advertiseService("set_imu_state", EN_IMU_SRV_CB); - ROS_INFO("REACHED9\n"); - en_odom = n.advertiseService("set_odom_state", EN_ODOM_SRV_CB); - ROS_INFO("REACHED10\n"); - reset_odom = n.advertiseService("reset_odom", RESET_ODOM_SRV_CB); - ROS_INFO("REACHED11\n"); + init_node(n); signal(SIGINT, sighandler); ros::spin(); diff --git a/qt_pi/qt_pi_serial/src/ros_node.cpp b/qt_pi/qt_pi_serial/src/ros_node.cpp index b70b3cf..18169cf 100644 --- a/qt_pi/qt_pi_serial/src/ros_node.cpp +++ b/qt_pi/qt_pi_serial/src/ros_node.cpp @@ -1,6 +1,26 @@ #include "qt_pi_serial/ros_node.h" -sensor_msgs::Imu data; +sensor_msgs::Imu imu_data; +unsigned char in_imu_data_buf[14]; + +qt_pi_msgs::Encoder prev_enc_data; +nav_msgs::Odometry odom_data; +int32_t in_enc_data_buf[2]; +float prev_vel_data[2]; +float theta; + +void init_odom(); + +void report_error(int ret){ + switch(ret){ + case 1: + ROS_ERROR("Wait for reply timed out."); + break; + case -1: + ROS_ERROR("Invalid packet received"); + break; + } +} bool GET_ENC_SRV_CB(qt_pi_msgs::GetEncoderData::Request &req, qt_pi_msgs::GetEncoderData::Response &res) { @@ -9,15 +29,18 @@ bool GET_ENC_SRV_CB(qt_pi_msgs::GetEncoderData::Request &req, com_buf[CIDB] = 1; com_buf[DATASTRB] = 1; - com_buf_len = 3; + com_buf_len = com_preamble_len + 1; int ret = sendCom(); if (ret) + { + report_error(ret); return false; + } memcpy(&res.data.LEFT, com_buf + DATASTRB, 4); - memcpy(&res.data.RIGHT, com_buf + DATASTRB, 4); + memcpy(&res.data.RIGHT, com_buf + DATASTRB + 4, 4); return true; } @@ -28,18 +51,23 @@ bool GET_BATT_SRV_CB(qt_pi_msgs::GetBatteryInfo::Request &req, com_buf[CIDB] = 1; com_buf[DATASTRB] = 1; - com_buf_len = 3; + com_buf_len = com_preamble_len + 1; int ret = sendCom(); - if (ret) + if (ret){ + report_error(ret); return false; + } uint16_t volts; memcpy(&volts, com_buf + DATASTRB, 2); - res.volts = volts / 100.0; + res.volts = volts * 11.14 / 876.0; + + return true; + } bool GET_PID_SRV_CB(qt_pi_msgs::GetPIDData::Request &req, @@ -49,21 +77,23 @@ bool GET_PID_SRV_CB(qt_pi_msgs::GetPIDData::Request &req, com_buf[CIDB] = 1; com_buf[DATASTRB] = 1; - com_buf_len = 3; + com_buf_len = com_preamble_len + 1; int ret = sendCom(); - if (ret) + if (ret){ + report_error(ret); return false; + } - res.data.lkp = com_buf[DATASTRB]; - res.data.lkd = com_buf[DATASTRB + 1]; - res.data.lki = com_buf[DATASTRB + 2]; - res.data.lko = com_buf[DATASTRB + 3]; - res.data.rkp = com_buf[DATASTRB + 4]; - res.data.rkd = com_buf[DATASTRB + 5]; - res.data.rki = com_buf[DATASTRB + 6]; - res.data.rko = com_buf[DATASTRB + 7]; + res.LEFT.kp = com_buf[DATASTRB]; + res.LEFT.kd = com_buf[DATASTRB + 1]; + res.LEFT.ki = com_buf[DATASTRB + 2]; + res.LEFT.ko = com_buf[DATASTRB + 3]; + res.RIGHT.kp = com_buf[DATASTRB + 4]; + res.RIGHT.kd = com_buf[DATASTRB + 5]; + res.RIGHT.ki = com_buf[DATASTRB + 6]; + res.RIGHT.ko = com_buf[DATASTRB + 7]; return true; } @@ -74,121 +104,484 @@ bool GET_BAUD_SRV_CB(qt_pi_msgs::GetBaudRate::Request &req, com_buf[CIDB] = 1; com_buf[DATASTRB] = 1; - com_buf_len = 3; - ROS_INFO("CALLED\n"); + com_buf_len = com_preamble_len + 1; int ret = sendCom(); - ROS_INFO("CALLED\n"); - if (ret) + if (ret){ + report_error(ret); return false; + } - ROS_INFO("CALLED\n"); memcpy(&(res.BAUDRATE), com_buf + DATASTRB, 4); - ROS_INFO("CALLED\n"); return true; } bool SET_PID_SRV_CB(qt_pi_msgs::SetPIDData::Request &req, qt_pi_msgs::SetPIDData::Response &res) { - com_buf[COMB] = GET_PID_P; + if(req.motor == LEFT_WHEEL) + com_buf[COMB] = SET_PID_L; + else + com_buf[COMB] = SET_PID_R; + com_buf[CIDB] = 1; - com_buf[DATASTRB] = req.data.lkp; - com_buf[DATASTRB + 1] = req.data.lkd; - com_buf[DATASTRB + 2] = req.data.lki; - com_buf[DATASTRB + 3] = req.data.lko; - com_buf[DATASTRB + 4] = req.data.rkp; - com_buf[DATASTRB + 5] = req.data.rkd; - com_buf[DATASTRB + 6] = req.data.rki; - com_buf[DATASTRB + 7] = req.data.rko; + com_buf[DATASTRB] = req.data.kp; + com_buf[DATASTRB + 1] = req.data.kd; + com_buf[DATASTRB + 2] = req.data.ki; + com_buf[DATASTRB + 3] = req.data.ko; - com_buf_len = 10; + com_buf_len = 6; int ret = sendCom(); - if (ret) + if (ret){ + report_error(ret); return false; + } return true; } + bool EN_IMU_SRV_CB(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) { com_buf[COMB] = EN_IMU; com_buf[CIDB] = 1; com_buf[DATASTRB] = req.data; - com_buf_len = 3; + com_buf_len = com_preamble_len + 1; res.success = 0; int ret = sendCom(); - if (ret) + if (ret){ + report_error(ret); return false; - + } + res.success = 1; return true; } + bool EN_ODOM_SRV_CB(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) { com_buf[COMB] = EN_ODOM; com_buf[CIDB] = 1; com_buf[DATASTRB] = req.data; - com_buf_len = 3; + com_buf_len = com_preamble_len + 1; res.success = 0; int ret = sendCom(); - if (ret) + if (ret){ + report_error(ret); return false; + } res.success = 1; return true; } + bool RESET_ODOM_SRV_CB(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { com_buf[COMB] = RESET_ENCODERS; com_buf[CIDB] = 1; com_buf[DATASTRB] = 1; - com_buf_len = 3; + com_buf_len = com_preamble_len + 1; int ret = sendCom(); res.success = 0; - if (ret) + if (ret){ + report_error(ret); return false; + } + + init_odom(); res.success = 1; return true; } -void ODOM_DATA_PUBLISH(unsigned char *buf) {} +void init_odom(){ + + memset(in_enc_data_buf, 0, 8); + memset(&prev_enc_data, 0, sizeof(qt_pi_msgs::Encoder)); + memset(&odom_data, 0, sizeof(nav_msgs::Odometry)); + memset(prev_vel_data, 0 , sizeof(float) * 2); + + theta = 0; + + odom_data.pose.pose.position.x = 0; + odom_data.pose.pose.position.y = 0; + odom_data.pose.pose.position.z = 0; + + double cov_pose[36] = {0.05, 0, 0, 0, 0, 0, + 0, 0.05, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.3}; + + double cov_twist[36] = {0.01, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.03}; + + memcpy(&(odom_data.pose.covariance), cov_pose, 36 * sizeof(double)); + memcpy(&(odom_data.twist.covariance), cov_twist, 36 * sizeof(double)); + + odom_data.pose.pose.orientation.x = 0; + odom_data.pose.pose.orientation.y = 0; + odom_data.pose.pose.orientation.z = 0; + odom_data.pose.pose.orientation.w = 1; + + odom_data.twist.twist.linear.x = 0; + odom_data.twist.twist.linear.y = 0; + odom_data.twist.twist.linear.z = 0; + + odom_data.twist.twist.angular.x = 0; + odom_data.twist.twist.angular.y = 0; + odom_data.twist.twist.angular.z = 0; + + odom_data.header.stamp = ros::Time::now(); + odom_data.header.frame_id = "odom"; + odom_data.child_frame_id = "base_link"; +} + +void ODOM_DATA_PUBLISH(unsigned char *buf) { + + static tf::TransformBroadcaster br; + + memcpy(in_enc_data_buf, buf, 8); + + float diff_disp[2]; + float dx, dy, dtheta, ddisp, vl, vr; + + diff_disp[LEFT_WHEEL] = M_PI * wheel_dia * (in_enc_data_buf[LEFT_WHEEL] - prev_enc_data.LEFT ) / (gear_ratio * counts_per_rot); + diff_disp[RIGHT_WHEEL] = M_PI * wheel_dia * (in_enc_data_buf[RIGHT_WHEEL] - prev_enc_data.RIGHT) / (gear_ratio * counts_per_rot); + + ddisp = ( diff_disp[LEFT_WHEEL] + diff_disp[RIGHT_WHEEL] ) / 2.0; + dtheta = ( diff_disp[RIGHT_WHEEL] - diff_disp[LEFT_WHEEL] ) / wheel_sep; + + dx = ddisp * cos(theta + dtheta); //Check if dtheta will be halved or not + dy = ddisp * sin(theta + dtheta); + + theta += dtheta; + + odom_data.pose.pose.position.x += dx; + odom_data.pose.pose.position.y += dy; + + odom_data.pose.pose.orientation.z = sin( theta / 2.0 ); + odom_data.pose.pose.orientation.w = cos( theta / 2.0 ); + + prev_enc_data.LEFT = in_enc_data_buf[LEFT_WHEEL]; + prev_enc_data.RIGHT = in_enc_data_buf[RIGHT_WHEEL]; + + vl = diff_disp[LEFT_WHEEL] * ODOM_RATE; + vr = diff_disp[RIGHT_WHEEL] * ODOM_RATE; + + vl = vl * (1 - v_alpha) + prev_vel_data[LEFT_WHEEL] * v_alpha; + vr = vr * (1 - v_alpha) + prev_vel_data[RIGHT_WHEEL] * v_alpha; + + prev_vel_data[LEFT_WHEEL] = vl; + prev_vel_data[RIGHT_WHEEL] = vr; + + odom_data.twist.twist.linear.x = ( vr + vl ) / 2.0; + odom_data.twist.twist.angular.z = ( vr - vl ) / 2.0; + + odom_data.header.stamp = ros::Time::now(); + + odom_pub.publish(odom_data); + + br.sendTransform( + tf::StampedTransform( + tf::Transform( + tf::Quaternion( + odom_data.pose.pose.orientation.x, + odom_data.pose.pose.orientation.y, + odom_data.pose.pose.orientation.z, + odom_data.pose.pose.orientation.w + ), + tf::Vector3( + odom_data.pose.pose.position.x, + odom_data.pose.pose.position.y, + odom_data.pose.pose.position.z + ) + ), + ros::Time::now(), + "odom", + "base_link" + ) + ); + + br.sendTransform( + tf::StampedTransform( + tf::Transform( + tf::Quaternion( + 0, + 0, + 0, + 1 + ), + tf::Vector3( + 0.076, + -0.09 , + 0.15 + ) + ), + ros::Time::now(), + "base_link", + "camera_link" + ) + ); + + br.sendTransform( + tf::StampedTransform( + tf::Transform( + tf::Quaternion( + 0, + 0, + -0.70710678119, + 0.70710678119 + ), + tf::Vector3( + -0.025, + -0.05 , + 0.085 + ) + ), + ros::Time::now(), + "base_link", + "imu_link" + ) + ); +} + +void TARE_CALLBACK(unsigned char *buf){ + + static int count = 0; + static int32_t imu_off_ave[] = {0, 0, 0, 0, 0, 0}; + + memcpy(in_imu_data_buf, buf, 14); + + int16_t imu_raw[6]; + + imu_raw[0] = ((int16_t)in_imu_data_buf[0] << 8 ) | in_imu_data_buf[1]; + imu_raw[1] = ((int16_t)in_imu_data_buf[2] << 8 ) | in_imu_data_buf[3]; + imu_raw[2] = ((int16_t)in_imu_data_buf[4] << 8 ) | in_imu_data_buf[5]; + + imu_raw[3] = ((int16_t)in_imu_data_buf[ 8] << 8) | in_imu_data_buf[ 9]; + imu_raw[4] = ((int16_t)in_imu_data_buf[10] << 8) | in_imu_data_buf[11]; + imu_raw[5] = ((int16_t)in_imu_data_buf[12] << 8) | in_imu_data_buf[13]; + + if(count == 200) + { + + for(int i = 0; i < 6; i++) + { + imu_off_ave[i] /= count; + imu_offset[i] = (int16_t)(imu_off_ave[i]); + } + + imu_offset[2] -= 16384; + + set_CB(&IMU_DATA_PUBLISH); + return; + } + + for(int i = 0; i < 6; i++) + { + imu_off_ave[i] += imu_raw[i]; + } + + count++; +} + +void tare_routine(){ + + com_buf[COMB] = EN_IMU; + com_buf[CIDB] = 1; + com_buf[DATASTRB] = 0; + + com_buf_len = com_preamble_len + 1; + + int ret = sendCom(); + + if (ret){ + report_error(ret); + ROS_ERROR("Could not disable IMU. Returning."); + return; + } + + set_CB(&TARE_CALLBACK); + + com_buf[COMB] = EN_IMU; + com_buf[CIDB] = 1; + com_buf[DATASTRB] = 1; + + com_buf_len = com_preamble_len + 1; + + ret = sendCom(); + + if (ret){ + report_error(ret); + ROS_ERROR("Could not enable IMU. Setting normal imu CB and returning."); + set_CB(&IMU_DATA_PUBLISH); + return; + } +} + +void IMU_TARE_CMD_CB(const std_msgs::Bool::ConstPtr& msg ){ + tare_routine(); +} void IMU_DATA_PUBLISH(unsigned char *buf) { - int16_t tmp; + imu_data.header.stamp = ros::Time::now(); + imu_data.header.frame_id = "imu_link"; - memcpy(&tmp, buf, 2); - data.linear_acceleration.x = tmp * 9.8 / 16384.0; - memcpy(&tmp, buf + 2, 2); - data.linear_acceleration.y = tmp * 9.8 / 16384.0; - memcpy(&tmp, buf + 4, 2); - data.linear_acceleration.z = tmp * 9.8 / 16384.0; + memcpy(in_imu_data_buf, buf, 14); - memcpy(&tmp, buf + 8, 2); - data.angular_velocity.x = tmp * (M_PI / 180.0) / 131.0; - memcpy(&tmp, buf + 10, 2); - data.angular_velocity.y = tmp * (M_PI / 180.0) / 131.0; - memcpy(&tmp, buf + 12, 2); - data.angular_velocity.z = tmp * (M_PI / 180.0) / 131.0; + int16_t imu_raw[6]; + + imu_raw[0] = ((int16_t)in_imu_data_buf[0] << 8 ) | in_imu_data_buf[1]; + imu_raw[1] = ((int16_t)in_imu_data_buf[2] << 8 ) | in_imu_data_buf[3]; + imu_raw[2] = ((int16_t)in_imu_data_buf[4] << 8 ) | in_imu_data_buf[5]; - imu_pub.publish(data); + imu_raw[3] = ((int16_t)in_imu_data_buf[ 8] << 8) | in_imu_data_buf[ 9]; + imu_raw[4] = ((int16_t)in_imu_data_buf[10] << 8) | in_imu_data_buf[11]; + imu_raw[5] = ((int16_t)in_imu_data_buf[12] << 8) | in_imu_data_buf[13]; + + imu_data.orientation_covariance[0] = -1; + + imu_data.linear_acceleration.x = (imu_raw[0] - imu_offset[0]) * 9.8 / 16384.0; + imu_data.linear_acceleration.y = (imu_raw[1] - imu_offset[1]) * 9.8 / 16384.0; + imu_data.linear_acceleration.z = (imu_raw[2] - imu_offset[2]) * 9.8 / 16384.0; + + imu_data.linear_acceleration_covariance[0] = imu_data.linear_acceleration_covariance[4] = imu_data.linear_acceleration_covariance[8] = 0.0392266; + imu_data.linear_acceleration_covariance[1] = imu_data.linear_acceleration_covariance[3] = imu_data.linear_acceleration_covariance[6] = 0.0; + imu_data.linear_acceleration_covariance[2] = imu_data.linear_acceleration_covariance[5] = imu_data.linear_acceleration_covariance[7] = 0.0; + + imu_data.angular_velocity.x = (imu_raw[3] - imu_offset[3]) * (M_PI / 180.0) / 131.0; + imu_data.angular_velocity.y = (imu_raw[4] - imu_offset[4]) * (M_PI / 180.0) / 131.0; + imu_data.angular_velocity.z = (imu_raw[5] - imu_offset[5]) * (M_PI / 180.0) / 131.0; + + imu_data.angular_velocity_covariance[0] = imu_data.angular_velocity_covariance[4] = imu_data.angular_velocity_covariance[8] = 0.0008726646; + imu_data.angular_velocity_covariance[1] = imu_data.angular_velocity_covariance[3] = imu_data.angular_velocity_covariance[6] = 0.0; + imu_data.angular_velocity_covariance[2] = imu_data.angular_velocity_covariance[5] = imu_data.angular_velocity_covariance[7] = 0.0; + + imu_pub.publish(imu_data); +} + +void CMD_VEL_CB(const geometry_msgs::Twist::ConstPtr &msg){ + + com_buf[COMB] = SET_SPEEDS; + com_buf[CIDB] = 1; + + double left = (msg->linear.x - msg->angular.z * wheel_sep / 2.0); + double right = (msg->linear.x + msg->angular.z * wheel_sep / 2.0); + + double ticks_per_frame_l = left * ( gear_ratio * counts_per_rot / ( M_PI * wheel_dia ) ) / ODOM_RATE; + double ticks_per_frame_r = right * ( gear_ratio * counts_per_rot / ( M_PI * wheel_dia ) ) / ODOM_RATE; + + int32_t l = ticks_per_frame_l * 1000; + int32_t r = ticks_per_frame_r * 1000; + + memcpy(com_buf + DATASTRB, &l, 4); + memcpy(com_buf + DATASTRB + 4, &r, 4); + + com_buf_len = com_preamble_len + 8; + + int ret = sendCom(); + + ROS_INFO("%f\t%f\n", ticks_per_frame_l, ticks_per_frame_r); + + if (ret){ + report_error(ret); + ROS_ERROR("Could not set motor speeds. Returning."); + return; + } + +} + +void init_node(ros::NodeHandle n){ + + ROS_INFO("Opening serial port."); + + if (init_comm("/dev/qt_pi_serial_port") < 0) { + ROS_FATAL("Unable to open port"); + exit(0); + } + + ROS_INFO("Port opened successfully. Initializing services."); + + pthread_yield(); + + usleep(2000000); + + imu_pub = n.advertise("imu", 1); + odom_pub = n.advertise("odom", 1); + + get_baudrate = n.advertiseService("get_baudrate", GET_BAUD_SRV_CB); + get_batt_state = n.advertiseService("get_battery_state", GET_BATT_SRV_CB); + + get_encoder = n.advertiseService("get_encoder_state", GET_ENC_SRV_CB); + reset_odom = n.advertiseService("reset_odom", RESET_ODOM_SRV_CB); + + get_pid_val = n.advertiseService("get_pid_values", GET_PID_SRV_CB); + set_pid_val = n.advertiseService("set_pid_values", SET_PID_SRV_CB); + + imu_tare = n.subscribe("imu_tare", 1, IMU_TARE_CMD_CB); + en_imu = n.advertiseService("set_imu_state", EN_IMU_SRV_CB); + en_odom = n.advertiseService("set_odom_state", EN_ODOM_SRV_CB); + + move_cmd = n.subscribe("cmd_vel", 1, CMD_VEL_CB); + + ROS_INFO("Services initialized"); + + tare_routine(); + init_odom(); +} + +void shutdown_node(){ + + com_buf[COMB] = EN_IMU; + com_buf[CIDB] = 1; + com_buf[DATASTRB] = 0; + + com_buf_len = com_preamble_len + 1; + + int ret = sendCom(); + + if (ret){ + report_error(ret); + ROS_ERROR("Could not disable IMU. Shutting down."); + return; + } + + + com_buf[COMB] = EN_ODOM; + com_buf[CIDB] = 1; + com_buf[DATASTRB] = 0; + + com_buf_len = com_preamble_len + 1; + + ret = sendCom(); + + if (ret){ + report_error(ret); + ROS_ERROR("Could not disable ODOM. Shutting down."); + return; + } } diff --git a/qt_pi/qt_pi_serial/src/serial_driver.cpp b/qt_pi/qt_pi_serial/src/serial_driver.cpp index 53dcbe6..d569275 100644 --- a/qt_pi/qt_pi_serial/src/serial_driver.cpp +++ b/qt_pi/qt_pi_serial/src/serial_driver.cpp @@ -1,5 +1,4 @@ #include "qt_pi_serial/serial_driver.h" -#include "stdio.h" unsigned char com_buf[buf_size]; int com_buf_len; @@ -9,7 +8,7 @@ int sig = 1; pthread_t thread_id; int fd; -unsigned char in_out_buf[buf_size]; +unsigned char out_buf[buf_size]; int payload_len; com_state state = IDLE; @@ -21,6 +20,12 @@ struct timespec ts; int open_port(const char *); void *serialDataIn(void *); +void (*IMU_DATA_PUBLISH_CB)(unsigned char *) = NULL; + +void set_CB(void (*pub)(unsigned char *)){ + IMU_DATA_PUBLISH_CB = pub; +} + int init_comm(const char *port) { if ((fd = open_port(port)) < 0) @@ -54,12 +59,20 @@ int open_port(const char *port) { options.c_cflag &= ~PARENB; options.c_cflag &= ~CSIZE; options.c_cflag &= ~CSTOPB; - options.c_cflag &= ~HUPCL; options.c_cflag |= CS8; + options.c_cflag &= ~CRTSCTS; + options.c_cc[VTIME] = 5; options.c_cc[VMIN] = 1; - if (tcsetattr(fd, TCSANOW, &options)) { + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + + options.c_iflag &= ~(IXON | IXOFF | IXANY); + + options.c_oflag &= ~OPOST; + + tcsetattr(fd, TCSANOW, &options); + if (tcsetattr(fd, TCSAFLUSH, &options)) { close(fd); return -1; } @@ -79,7 +92,8 @@ void runRecvCom(uint16_t com_len, unsigned char *com) { case EN_IMU: case SET_PID_L: case SET_PID_R: - + case RESET_ENCODERS: + case SET_SPEEDS: if (state != COM_SENT) return; @@ -97,7 +111,7 @@ void runRecvCom(uint16_t com_len, unsigned char *com) { case IMU_DATA: - IMU_DATA_PUBLISH(com + DATASTRB); + IMU_DATA_PUBLISH_CB(com + DATASTRB); break; @@ -106,6 +120,11 @@ void runRecvCom(uint16_t com_len, unsigned char *com) { ODOM_DATA_PUBLISH(com + DATASTRB); break; + + default: + + com_buf_len = -1; + } } @@ -205,25 +224,21 @@ void *serialDataIn(void *vargp) { } int sendCom() { - - if (state != IDLE) - return 2; - int data_len = com_buf_len - com_preamble_len; - in_out_buf[MB1P] = magic_start1; - in_out_buf[MB2P] = magic_start2; - memcpy(in_out_buf + LB1P, &com_buf_len, len_len); - in_out_buf[CIDBP] = com_buf[CIDB]; - in_out_buf[COMBP] = com_buf[COMB]; - memcpy(in_out_buf + DATASTRBP, com_buf + DATASTRB, data_len); - in_out_buf[preamble_len + com_buf_len] = magic_end; + out_buf[MB1P] = magic_start1; + out_buf[MB2P] = magic_start2; + memcpy(out_buf + LB1P, &com_buf_len, len_len); + out_buf[CIDBP] = com_buf[CIDB]; + out_buf[COMBP] = com_buf[COMB]; + memcpy(out_buf + DATASTRBP, com_buf + DATASTRB, data_len); + out_buf[preamble_len + com_buf_len] = magic_end; pthread_mutex_lock(&data_wait_mutex); state = COM_SENT; - write(fd, in_out_buf, net_extra_len + com_buf_len); + write(fd, out_buf, net_extra_len + com_buf_len); clock_gettime(CLOCK_REALTIME, &ts); ts.tv_sec += 2; @@ -233,15 +248,17 @@ int sendCom() { ETIMEDOUT) { pthread_mutex_unlock(&data_wait_mutex); + state = IDLE; return 1; } } - memcpy(com_buf, in_out_buf + preamble_len, com_buf_len); - - pthread_mutex_unlock(&data_wait_mutex); + pthread_mutex_unlock(&data_wait_mutex); state = IDLE; + + if(com_buf_len == -1) + return -1; return 0; } @@ -250,7 +267,7 @@ void close_comm() { sig = 0; pthread_yield(); pthread_cancel(thread_id); - pthread_join(thread_id, nullptr); + pthread_join(thread_id, NULL); pthread_mutex_destroy(&data_wait_mutex); pthread_cond_destroy(&data_wait_cond); close(fd); diff --git a/qt_pi/qt_pi_serial/tests/comm_test.cpp b/qt_pi/qt_pi_serial/tests/comm_test.cpp index 422fb8f..7d61a37 100644 --- a/qt_pi/qt_pi_serial/tests/comm_test.cpp +++ b/qt_pi/qt_pi_serial/tests/comm_test.cpp @@ -29,7 +29,7 @@ int open_port(const char *port) { return -1; } else fcntl(fd, F_SETFL, 0); - + usleep(2000000); struct termios options; tcgetattr(fd, &options); @@ -41,15 +41,25 @@ int open_port(const char *port) { options.c_cflag &= ~PARENB; options.c_cflag &= ~CSIZE; options.c_cflag &= ~CSTOPB; - options.c_cflag &= ~HUPCL; options.c_cflag |= CS8; + options.c_cflag &= ~CRTSCTS; + options.c_cc[VTIME] = 5; options.c_cc[VMIN] = 1; - if (tcsetattr(fd, TCSANOW, &options)) { + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + + options.c_iflag &= ~(IXON | IXOFF | IXANY); + + options.c_oflag &= ~OPOST; + + tcsetattr(fd, TCSANOW, &options); + if (tcsetattr(fd, TCSAFLUSH, &options)) { close(fd); return -1; } + usleep(2000000); + return (fd); } @@ -72,7 +82,6 @@ void *serialDataIn(void *vargp) { pthread_yield(); while (1) { - printf("Waiting\n"); inL = read(fd, &inChar, 1); printf("%x\n", inChar); } @@ -99,7 +108,6 @@ int main(int argc, char **argv) { } l = strlen(in_string) / 2; - printf("%d\n", l); hex2bin(in_string, 2 * l, buffer); int n = write(fd, buffer, l); diff --git a/qt_pi/qt_pi_serial/tests/imu_data_test.cpp b/qt_pi/qt_pi_serial/tests/imu_data_test.cpp new file mode 100644 index 0000000..8951397 --- /dev/null +++ b/qt_pi/qt_pi_serial/tests/imu_data_test.cpp @@ -0,0 +1,187 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +int sig = 1; +int open_port(const char *port) { + int fd; + + fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY); + if (fd == -1) { + return -1; + } else + fcntl(fd, F_SETFL, 0); + usleep(2000000); + struct termios options; + + tcgetattr(fd, &options); + + cfsetispeed(&options, B115200); + cfsetospeed(&options, B115200); + + options.c_cflag |= (CLOCAL | CREAD); + options.c_cflag &= ~PARENB; + options.c_cflag &= ~CSIZE; + options.c_cflag &= ~CSTOPB; + options.c_cflag |= CS8; + options.c_cc[VTIME] = 5; + options.c_cc[VMIN] = 1; + + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + usleep(2000000); + + if (tcsetattr(fd, TCSAFLUSH, &options)) { + close(fd); + return -1; + } + + return (fd); +} + +pthread_t thread_id; +int fd; + +void *serialDataIn(void *); + +void *serialDataIn(void *vargp) { + + enum PACKET_STATE { + WAITING_FOR_NEW_COM, + FIRST_BYTE_VERIFIED, + SECOND_BYTE_VERIFIED, + LENGTH_INPUT, + READING_PAYLOAD, + READING_COMPLETE, + COM_RECEIVED + }; + + int inL = 0; + + unsigned char com_recv[25]; + unsigned char in_buf[25]; + unsigned char inChar; + int in_buf_i = 0; + uint16_t com_len; + + PACKET_STATE curState = WAITING_FOR_NEW_COM; + pthread_yield(); + + while (sig) { + + inL = read(fd, &inChar, 1); + + switch (curState) { + + case WAITING_FOR_NEW_COM: + + if (inChar == magic_start1) { + curState = FIRST_BYTE_VERIFIED; + } + + break; + + case FIRST_BYTE_VERIFIED: + + if (inChar == magic_start2) { + curState = SECOND_BYTE_VERIFIED; + } else { + curState = WAITING_FOR_NEW_COM; + } + + break; + + case SECOND_BYTE_VERIFIED: + + com_len = inChar; + curState = LENGTH_INPUT; + + break; + + case LENGTH_INPUT: + + com_len = com_len | (inChar << 8); + + if (com_len < 20) { + in_buf_i = 0; + curState = READING_PAYLOAD; + } else { + curState = WAITING_FOR_NEW_COM; + } + + break; + + case READING_PAYLOAD: + + in_buf[in_buf_i] = inChar; + in_buf_i++; + + if (in_buf_i == com_len) { + curState = READING_COMPLETE; + } + break; + + case READING_COMPLETE: + + in_buf_i = 0; + + if (inChar == magic_end) { + + memcpy(com_recv, in_buf, com_len); + + int16_t tmp[6]; + float imu[6]; + + memcpy(tmp, in_buf, 2); + memcpy(tmp+1, in_buf + 2, 2); + memcpy(tmp+2, in_buf + 4, 2); + + memcpy(tmp+3, in_buf + 8, 2); + memcpy(tmp+4, in_buf + 10, 2); + memcpy(tmp+5, in_buf + 12, 2); + +for(int i = 0; i < 3;i++) +{ + imu[i] = tmp[i] * 9.8 / 16384.0; + imu[i + 3] = tmp[i + 3] * (M_PI / 180.0) / 131.0; +} + + printf("%f \t %f \t %f \t %f \t %f \t %f\n", imu[0], imu[1], imu[2], imu[3], imu[4], imu[5]); + } + + curState = WAITING_FOR_NEW_COM; + + break; + } + } +} + + +int main(int argc, char **argv) { + fd = open_port(argv[1]); + if (fd < 0) + return 0; + + pthread_create(&thread_id, NULL, serialDataIn, NULL); + pthread_yield(); + int l = 8; + unsigned char buffer[] = {0xee, 0xae, 0x03, 0x00, 0x59, 0x59, 0x01, 0xfc}; + + int n = write(fd, buffer, l); + char in_string[64]; + while (1) { + + fgets(in_string, 64, stdin); + if (in_string[0] == 'q') { + sig = 0; + pthread_cancel(thread_id); + close(fd); + return 0; + } + + } +}