yikestone/qt_pi
1
0
Fork 0
mirror of https://gitlab.com/yikestone/qt_pi.git synced 2025-08-03 21:54:12 +05:30
Code Issues Projects Releases Wiki Activity

Hopefully no bugs in firmware.

Browse Source
This commit is contained in:
Rishabh Kundu 2020-10-30 15:34:32 +05:30
parent 7255417d7a
commit c4cae9f087
34 changed files with 931 additions and 264 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
qt_pi/qt_pi/CMakeLists.txt Normal file
View File

@ -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()

6
qt_pi/qt_pi/launch/arduino.launch
View File

@ -1,6 +0,0 @@
<launch>
<node name="arduino" pkg="qt_pi" type="arduino_node.py" output="screen">
<rosparam file="$(find qt_pi)/config/qt_pi_arduino_params.yaml" command="load" />
</node>
</launch>

24
qt_pi/qt_pi/launch/localization.launch Normal file
View File

@ -0,0 +1,24 @@
<launch>
<node name="localization" pkg="robot_localization" type="ekf_localization_node" clear_params="true">
<param name="frequency" value="15." />
<param name="sensor_timeout" value="0.2" />
<param name="two_d_mode" value="true" />
<param name="publish_tf" value="true" />
<param name="odom_frame" value="odom" />
<param name="base_link_frame" value="base_link" />
<param name="imu_link_frame" value="imu_link" />
<param name="imu0" value="imu"/>
<param name="odom0" value="odom"/>
<rosparam param="odom0_config"> [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false ]</rosparam>
<rosparam param="imu0_config"> [false, false, false,
false, false, false,
false, false, false,
false, false, true,
true, false, false ]</rosparam>
</node>
</launch>

9
qt_pi/qt_pi/launch/qt_pi.launch
View File

@ -1,10 +1,5 @@
<launch>
<include file="$(find kinect2_bridge)/launch/kinect2_bridge.launch"/>
<include file="$(find qt_pi)/launch/arduino.launch"/>
<include file="$(find qt_pi)/launch/depth_to_laser.launch"/>
<include file="$(find qt_pi)/launch/gmapping.launch"/>
<include file="$(find qt_pi_serial)/launch/serial.launch"/>
<include file="$(find qt_pi)/launch/localization.launch"/>
</launch>

25
qt_pi/qt_pi/package.xml Normal file
View File

@ -0,0 +1,25 @@
<?xml version="1.0"?>
<package format="2">
<name>qt_pi</name>
<version>0.0.0</version>
<description>The qt_pi package</description>
<maintainer email="rishabh_kundu@rediffmail.com">Rishabh Kundu</maintainer>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>kinect2_bridge</build_depend>
<build_depend>qt_pi_serial</build_depend>
<build_depend>robot_localization</build_depend>
<build_export_depend>kinect2_bridge</build_export_depend>
<build_export_depend>qt_pi_serial</build_export_depend>
<build_export_depend>robot_localization</build_export_depend>
<exec_depend>kinect2_bridge</exec_depend>
<exec_depend>qt_pi_serial</exec_depend>
<exec_depend>robot_localization</exec_depend>
<export>
</export>
</package>

15
qt_pi/qt_pi_embedded/src/embedded_firmware/batt.cpp
View File

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

2
qt_pi/qt_pi_embedded/src/embedded_firmware/batt.h
View File

@ -3,7 +3,7 @@
#define BATT_PIN A0
const int BATT_INTERVAL = 1000;
const int BATT_INTERVAL = 30000;
extern void initBatt();
extern void updateBattLevel();

33
qt_pi/qt_pi_embedded/src/embedded_firmware/comm.cpp
View File

@ -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)
{

2
qt_pi/qt_pi_embedded/src/embedded_firmware/comm.h
View File

@ -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

2
qt_pi/qt_pi_embedded/src/embedded_firmware/commands.h
View File

@ -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

55
qt_pi/qt_pi_embedded/src/embedded_firmware/diff_controller.cpp
View File

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

22
qt_pi/qt_pi_embedded/src/embedded_firmware/embedded_firmware.ino
View File

@ -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) {

34
qt_pi/qt_pi_embedded/src/embedded_firmware/encoder_driver.cpp
View File

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

35
qt_pi/qt_pi_embedded/src/embedded_firmware/imu.cpp
View File

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

8
qt_pi/qt_pi_embedded/src/embedded_firmware/imu.h
View File

@ -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;

12
qt_pi/qt_pi_embedded/src/embedded_firmware/motor_driver.cpp
View File

@ -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

1
qt_pi/qt_pi_embedded/src/embedded_firmware/qt_pi.h
View File

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

4
qt_pi/qt_pi_msgs/msg/Encoder.msg
View File

@ -1,2 +1,2 @@
int64 LEFT
int64 RIGHT
int32 LEFT
int32 RIGHT

12
qt_pi/qt_pi_msgs/msg/PID.msg
View File

@ -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

4
qt_pi/qt_pi_msgs/srv/GetPIDData.srv
View File

@ -1,2 +1,4 @@
---
qt_pi_msgs/PID data
qt_pi_msgs/PID LEFT
qt_pi_msgs/PID RIGHT

1
qt_pi/qt_pi_msgs/srv/SetPIDData.srv
View File

@ -1,2 +1,3 @@
int16 motor
qt_pi_msgs/PID data
---

3
qt_pi/qt_pi_serial/CMakeLists.txt
View File

@ -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)

0
qt_pi/qt_pi/config/qt_pi_arduino_params.yaml → qt_pi/qt_pi_serial/config/qt_pi_arduino_params.yaml
View File

2
qt_pi/qt_pi_serial/include/qt_pi_serial/commands.h
View File

@ -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

12
qt_pi/qt_pi_serial/include/qt_pi_serial/qt_pi_params.h Normal file
View File

@ -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

53
qt_pi/qt_pi_serial/include/qt_pi_serial/ros_node.h
View File

@ -6,41 +6,42 @@
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/UInt32.h>
#include <std_msgs/Bool.h>
#include <std_srvs/SetBool.h>
#include <std_srvs/Trigger.h>
#include <std_srvs/Empty.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Twist.h>
#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

1
qt_pi/qt_pi_serial/include/qt_pi_serial/serial_driver.h
View File

@ -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

5
qt_pi/qt_pi_serial/launch/serial.launch Normal file
View File

@ -0,0 +1,5 @@
<launch>
<node name="serial_node" pkg="qt_pi_serial" type="serial_node" output="screen">
<rosparam file="$(find qt_pi_serial)/config/qt_pi_arduino_params.yaml" command="load" />
</node>
</launch>

2
qt_pi/qt_pi_serial/package.xml
View File

@ -14,6 +14,7 @@
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>tf</build_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>qt_pi_msgs</exec_depend>
@ -22,4 +23,5 @@
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>std_srvs</exec_depend>
<exec_depend>tf</exec_depend>
</package>

31
qt_pi/qt_pi_serial/src/main.cpp
View File

@ -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<sensor_msgs::Imu>("imu", 1);
ROS_INFO("REACHED2\n");
odom_pub = n.advertise<nav_msgs::Odometry>("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();

503
qt_pi/qt_pi_serial/src/ros_node.cpp
View File

@ -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<sensor_msgs::Imu>("imu", 1);
odom_pub = n.advertise<nav_msgs::Odometry>("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;
}
}

61
qt_pi/qt_pi_serial/src/serial_driver.cpp
View File

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

18
qt_pi/qt_pi_serial/tests/comm_test.cpp
View File

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

187
qt_pi/qt_pi_serial/tests/imu_data_test.cpp Normal file
View File

@ -0,0 +1,187 @@
#include <fcntl.h>
#include <pthread.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <qt_pi_serial/payload_specs.h>
#include <math.h>
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;
}
}
}