mirror of
https://gitlab.com/yikestone/qt_pi.git
synced 2025-08-03 21:54:12 +05:30
Changed Arduino Code to support multiple commands simultaneously. Working on ROS node.
This commit is contained in:
Rishabh Kundu
parent
f4dd374b00
commit
b6f9a5dc1b
@ -1,5 +1,5 @@
|
||||
|
||||
port: /dev/rfcomm1
|
||||
port: /dev/ttyUSB0
|
||||
baud: 57600
|
||||
timeout: 0.1
|
||||
|
||||
|
||||
35
include/qt_pi/commands.h
Normal file
35
include/qt_pi/commands.h
Normal file
@ -0,0 +1,35 @@
|
||||
#ifndef COMMANDS_H
|
||||
#define COMMANDS_H
|
||||
|
||||
#define INVALID 0xFF
|
||||
#define INVALID_LENGTH 0xFE
|
||||
|
||||
#define GET_BAUDRATE 0x51
|
||||
#define GET_ENCODERS 0x52
|
||||
#define SET_SPEEDS 0x53
|
||||
#define RESET_ENCODERS 0x54
|
||||
#define SET_PID_L 0x55
|
||||
#define SET_PID_R 0x56
|
||||
#define GET_PID_P 0x57
|
||||
#define GET_BATTERY_LVL 0x58
|
||||
#define EN_IMU 0x59
|
||||
#define EN_ODOM 0x60
|
||||
#define ODOM_DATA 0x61
|
||||
#define IMU_DATA 0x62
|
||||
|
||||
#define GET_BAUDRATE_LEN 0x01
|
||||
#define GET_ENCODERS_LEN 0x01
|
||||
#define SET_SPEEDS_LEN 0x03
|
||||
#define RESET_ENCODERS_LEN 0x01
|
||||
#define SET_PID_LEN 0x05
|
||||
#define GET_PID_P_LEN 0x01
|
||||
#define GET_BATTERY_LVL_LEN 0x01
|
||||
#define EN_IMU_LEN 0x02
|
||||
#define EN_ODOM_LEN 0x02
|
||||
#define ODOM_DATA_LEN 0x08
|
||||
#define IMU_DATA_LEN 0x08
|
||||
|
||||
#define LEFT 0x00
|
||||
#define RIGHT 0x01
|
||||
|
||||
#endif
|
||||
27
include/qt_pi/payload_specs.h
Normal file
27
include/qt_pi/payload_specs.h
Normal file
@ -0,0 +1,27 @@
|
||||
#ifndef PAYLOAD_SPECS_H
|
||||
#define PAYLOAD_SPECS_H
|
||||
|
||||
#define MB1P 0
|
||||
#define MB2P 1
|
||||
#define LB1P 2
|
||||
#define LB2P 3
|
||||
#define CIDBP 4
|
||||
#define COMBP 5
|
||||
#define DATASTRBP 6
|
||||
|
||||
#define start_magic_len 2
|
||||
#define end_magic_len 1
|
||||
#define len_len 2
|
||||
|
||||
#define BID 0xFF
|
||||
|
||||
#define CIDB 0
|
||||
#define COMB 1
|
||||
#define DATASTRB 2
|
||||
|
||||
const int com_preamble_len = 2;
|
||||
|
||||
const int preamble_len = start_magic_len + len_len;
|
||||
const int net_extra_len = preamble_len + end_magic_len;
|
||||
|
||||
#endif
|
||||
9
include/qt_pi/serial_driver.h
Normal file
9
include/qt_pi/serial_driver.h
Normal file
@ -0,0 +1,9 @@
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <pthread.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
#include "commands.h"
|
||||
#include "payload_specs.h"
|
||||
7
launch/depth_to_laser.launch
Normal file
7
launch/depth_to_laser.launch
Normal file
@ -0,0 +1,7 @@
|
||||
<launch>
|
||||
<remap from="image" to="kinect2/sd/image_depth_rect"/>
|
||||
<node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" type="depthimage_to_laserscan">
|
||||
<param name="scan_height" value="1" />
|
||||
<param name="range_max" value="12" />
|
||||
</node>
|
||||
</launch>
|
||||
8
launch/gmapping.launch
Normal file
8
launch/gmapping.launch
Normal file
@ -0,0 +1,8 @@
|
||||
<launch>
|
||||
<node name="gmapping" pkg="gmapping" type="slam_gmapping">
|
||||
<param name="base_frame" value="base_link" type="string" />
|
||||
<param name="maxUrange" value="8" />
|
||||
<param name="maxRange" value="12" />
|
||||
<param name="minimumScore" value="50" />
|
||||
</node>
|
||||
</launch>
|
||||
@ -1,17 +1,10 @@
|
||||
<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>
|
||||
|
||||
<node name="kinect2_bridge_1455208236617866858" pkg="kinect2_bridge" type="kinect2_bridge">
|
||||
</node>
|
||||
<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"/>
|
||||
|
||||
<node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" type="depthimage_to_laserscan">
|
||||
<param name="image" value="kinect2/sd/image_depth_rect" />
|
||||
</node>
|
||||
|
||||
<node name="gmapping" pkg="gmapping" type="slam_gmapping">
|
||||
<param name="base_frame" value="base_link" type="string" />
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -1,7 +0,0 @@
|
||||
image: F404.pgm
|
||||
resolution: 0.050000
|
||||
origin: [-2000,-2000, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
|
||||
BIN
map/Untitled.pgm
BIN
map/Untitled.pgm
Binary file not shown.
@ -1,7 +0,0 @@
|
||||
image: F404.pgm
|
||||
resolution: 0.050000
|
||||
origin: [10,10, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@ -1,211 +1,96 @@
|
||||
#define BAUDRATE 57600
|
||||
#define MAX_PWM 255
|
||||
#include "qt_pi.h"
|
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include "WProgram.h"
|
||||
#endif
|
||||
#include "commands.h"
|
||||
#include "motor_driver.h"
|
||||
#include "encoder_driver.h"
|
||||
#include "diff_controller.h"
|
||||
#define _TASK_SLEEP_ON_IDLE_RUN
|
||||
#define _TASK_INLINE
|
||||
#define _TASK_TIMEOUT
|
||||
|
||||
float batt_lvl;
|
||||
#include <TaskScheduler.h>
|
||||
|
||||
#define PID_RATE 30 // Hz
|
||||
const int PID_INTERVAL = 1000 / PID_RATE;
|
||||
unsigned long nextPID = PID_INTERVAL;
|
||||
#define AUTO_STOP_INTERVAL 2000
|
||||
long lastMotorCommand = AUTO_STOP_INTERVAL;
|
||||
int arg = 0;
|
||||
int index = 0;
|
||||
char chr;
|
||||
char cmd;
|
||||
char argv1[16];
|
||||
char argv2[16];
|
||||
long arg1;
|
||||
long arg2;
|
||||
Task *batt_mon;
|
||||
Task *diff_control;
|
||||
Task *move_cmd_timeout;
|
||||
Task *odom_out;
|
||||
Task *imu_out;
|
||||
Task *ser_comm_timeout;
|
||||
|
||||
void resetCommand() {
|
||||
cmd = NULL;
|
||||
memset(argv1, 0, sizeof(argv1));
|
||||
memset(argv2, 0, sizeof(argv2));
|
||||
arg1 = 0;
|
||||
arg2 = 0;
|
||||
arg = 0;
|
||||
index = 0;
|
||||
}
|
||||
Scheduler ts;
|
||||
|
||||
int runCommand() {
|
||||
int i = 0;
|
||||
char *p = argv1;
|
||||
char *str;
|
||||
int pid_args[4];
|
||||
arg1 = atoi(argv1);
|
||||
arg2 = atoi(argv2);
|
||||
|
||||
switch(cmd) {
|
||||
case GET_BAUDRATE:
|
||||
Serial.println(BAUDRATE);
|
||||
break;
|
||||
|
||||
case READ_ENCODERS:
|
||||
Serial.print(readEncoder(LEFT));
|
||||
Serial.print(" ");
|
||||
Serial.println(readEncoder(RIGHT));
|
||||
break;
|
||||
|
||||
case RESET_ENCODERS:
|
||||
resetEncoders();
|
||||
resetPID();
|
||||
Serial.println("OK");
|
||||
break;
|
||||
|
||||
case MOTOR_SPEEDS:
|
||||
/* Reset the auto stop timer */
|
||||
lastMotorCommand = millis();
|
||||
if (arg1 == 0 && arg2 == 0) {
|
||||
setMotorSpeeds(0, 0);
|
||||
resetPID();
|
||||
moving = 0;
|
||||
}
|
||||
else moving = 1;
|
||||
leftPID.TargetTicksPerFrame = arg1;
|
||||
rightPID.TargetTicksPerFrame = arg2;
|
||||
Serial.println("OK");
|
||||
break;
|
||||
|
||||
case UPDATE_PID_L:
|
||||
while ((str = strtok_r(p, ":", &p)) != '\0') {
|
||||
pid_args[i] = atoi(str);
|
||||
i++;
|
||||
}
|
||||
leftPID.Kp = pid_args[0];
|
||||
leftPID.Kd = pid_args[1];
|
||||
leftPID.Ki = pid_args[2];
|
||||
leftPID.Ko = pid_args[3];
|
||||
Serial.println("OK");
|
||||
break;
|
||||
|
||||
case UPDATE_PID_R:
|
||||
while ((str = strtok_r(p, ":", &p)) != '\0') {
|
||||
pid_args[i] = atoi(str);
|
||||
i++;
|
||||
}
|
||||
rightPID.Kp = pid_args[0];
|
||||
rightPID.Kd = pid_args[1];
|
||||
rightPID.Ki = pid_args[2];
|
||||
rightPID.Ko = pid_args[3];
|
||||
Serial.println("OK");
|
||||
break;
|
||||
|
||||
case DISP_PID_P:
|
||||
Serial.println(leftPID.Kp);
|
||||
Serial.println(leftPID.Kd);
|
||||
Serial.println(leftPID.Ki);
|
||||
Serial.println(leftPID.Ko);
|
||||
Serial.println(rightPID.Kp);
|
||||
Serial.println(rightPID.Kd);
|
||||
Serial.println(rightPID.Ki);
|
||||
Serial.println(rightPID.Ko);
|
||||
Serial.println("OK");
|
||||
break;
|
||||
case BATTERY_LVL:
|
||||
Serial.println((int)(batt_lvl * 100));
|
||||
break;
|
||||
default:
|
||||
Serial.println("Invalid Command");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Setup function--runs once at startup. */
|
||||
void setup() {
|
||||
Serial.begin(BAUDRATE);
|
||||
|
||||
pinMode(A0,INPUT);
|
||||
digitalWrite(A0,1);
|
||||
batt_lvl = analogRead(A0);
|
||||
init_comm();
|
||||
|
||||
DDRC &= ~(1<<LEFT_ENC_PIN_A);
|
||||
DDRC &= ~(1<<LEFT_ENC_PIN_B);
|
||||
DDRD &= ~(1<<RIGHT_ENC_PIN_A);
|
||||
DDRD &= ~(1<<RIGHT_ENC_PIN_B);
|
||||
|
||||
//enable pull up resistors
|
||||
PORTC |= (1<<LEFT_ENC_PIN_A);
|
||||
PORTC |= (1<<LEFT_ENC_PIN_B);
|
||||
PORTD |= (1<<RIGHT_ENC_PIN_A);
|
||||
PORTD |= (1<<RIGHT_ENC_PIN_B);
|
||||
|
||||
// tell pin change mask to listen to left encoder pins
|
||||
PCMSK1 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
|
||||
// tell pin change mask to listen to right encoder pins
|
||||
PCMSK2 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
|
||||
|
||||
// enable PCINT1 and PCINT2 interrupt in the general interrupt mask
|
||||
PCICR |= (1 << PCIE1) | (1 << PCIE2);
|
||||
init_enc();
|
||||
|
||||
initMotorController();
|
||||
resetPID();
|
||||
|
||||
batt_mon = new Task(BATT_INTERVAL, TASK_FOREVER, &updateBattLevel, &ts, true, &initBatt, NULL);
|
||||
diff_control = new Task(PID_INTERVAL, TASK_FOREVER, &updatePID, &ts, true, &resetPID, NULL);
|
||||
move_cmd_timeout = new Task(AUTO_STOP_INTERVAL, 1, &moveTimeout, &ts, true, 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);
|
||||
|
||||
}
|
||||
|
||||
void loop() {
|
||||
void loop()
|
||||
{
|
||||
|
||||
batt_lvl = ( analogRead(A0) * 12.22 / 800.0 ) * 0.001 + batt_lvl * 0.999;
|
||||
ts.execute();
|
||||
proc_serial();
|
||||
|
||||
while (Serial.available() > 0) {
|
||||
|
||||
// Read the next character
|
||||
chr = Serial.read();
|
||||
|
||||
// Terminate a command with a CR
|
||||
if (chr == 13) {
|
||||
if (arg == 1) argv1[index] = NULL;
|
||||
else if (arg == 2) argv2[index] = NULL;
|
||||
runCommand();
|
||||
resetCommand();
|
||||
}
|
||||
// Use spaces to delimit parts of the command
|
||||
else if (chr == ' ') {
|
||||
// Step through the arguments
|
||||
if (arg == 0) arg = 1;
|
||||
else if (arg == 1) {
|
||||
argv1[index] = NULL;
|
||||
arg = 2;
|
||||
index = 0;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
else {
|
||||
if (arg == 0) {
|
||||
// The first arg is the single-letter command
|
||||
cmd = chr;
|
||||
}
|
||||
else if (arg == 1) {
|
||||
// Subsequent arguments can be more than one character
|
||||
argv1[index] = chr;
|
||||
index++;
|
||||
}
|
||||
else if (arg == 2) {
|
||||
argv2[index] = chr;
|
||||
index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (millis() > nextPID) {
|
||||
updatePID();
|
||||
nextPID += PID_INTERVAL;
|
||||
void move_cmd_timeout_reset()
|
||||
{
|
||||
// move_cmd_timeout->resetTimeout();
|
||||
}
|
||||
|
||||
// Check to see if we have exceeded the auto-stop interval
|
||||
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
|
||||
setMotorSpeeds(0, 0);
|
||||
moving = 0;
|
||||
resetPID();
|
||||
void enable_odom(bool a)
|
||||
{
|
||||
switch(a)
|
||||
{
|
||||
case 0:
|
||||
switch(odom_out->isEnabled())
|
||||
{
|
||||
case 1:
|
||||
odom_out->disable();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
switch(odom_out->isEnabled())
|
||||
{
|
||||
case 0:
|
||||
odom_out->enable();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void enable_imu(bool a)
|
||||
{
|
||||
switch(a)
|
||||
{
|
||||
case 0:
|
||||
switch(imu_out->isEnabled())
|
||||
{
|
||||
case 1:
|
||||
imu_out->disable();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
switch(imu_out->isEnabled())
|
||||
{
|
||||
case 0:
|
||||
imu_out->enable();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
26
src/libraries/Arduino/batt.cpp
Normal file
26
src/libraries/Arduino/batt.cpp
Normal file
@ -0,0 +1,26 @@
|
||||
#include "qt_pi.h"
|
||||
|
||||
float batt_lvl;
|
||||
|
||||
void initBatt(){
|
||||
|
||||
pinMode(BATT_PIN,INPUT);
|
||||
digitalWrite(BATT_PIN,1);
|
||||
|
||||
batt_lvl = analogRead(BATT_PIN);
|
||||
|
||||
}
|
||||
|
||||
void updateBattLevel(){
|
||||
|
||||
batt_lvl = ( analogRead(BATT_PIN) * 12.22 / 800.0 ) * 0.001 + batt_lvl * 0.999;
|
||||
|
||||
}
|
||||
|
||||
void getBattLvl(uint8_t *buf){
|
||||
|
||||
uint16_t tmp = (uint16_t)(batt_lvl * 100);
|
||||
|
||||
memcpy(buf, &tmp, 2);
|
||||
|
||||
}
|
||||
12
src/libraries/Arduino/batt.h
Normal file
12
src/libraries/Arduino/batt.h
Normal file
@ -0,0 +1,12 @@
|
||||
#ifndef BATT
|
||||
#define BATT
|
||||
|
||||
#define BATT_PIN A0
|
||||
|
||||
const int BATT_INTERVAL = 1000;
|
||||
|
||||
extern void initBatt();
|
||||
extern void updateBattLevel();
|
||||
extern void getBattLvl(uint8_t *);
|
||||
|
||||
#endif
|
||||
365
src/libraries/Arduino/comm.cpp
Normal file
365
src/libraries/Arduino/comm.cpp
Normal file
@ -0,0 +1,365 @@
|
||||
#include "qt_pi.h"
|
||||
|
||||
const uint8_t magic_start1 = 0xEE;
|
||||
const uint8_t magic_start2 = 0xAE;
|
||||
const uint8_t magic_end = 0xFC;
|
||||
|
||||
#define buf_size 32
|
||||
|
||||
/*
|
||||
#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,
|
||||
FIRST_BYTE_VERIFIED,
|
||||
SECOND_BYTE_VERIFIED,
|
||||
LENGTH_INPUT,
|
||||
READING_PAYLOAD,
|
||||
READING_COMPLETE,
|
||||
COM_RECEIVED
|
||||
};
|
||||
|
||||
int curState = WAITING_FOR_NEW_COM;
|
||||
|
||||
int com_len;
|
||||
|
||||
uint8_t in_buf[buf_size];
|
||||
int in_buf_i;
|
||||
|
||||
uint8_t ser_write_buf[buf_size];
|
||||
uint8_t com_recv[buf_size];
|
||||
|
||||
int init_comm(){
|
||||
|
||||
ser_write_buf[MB1P] = magic_start1;
|
||||
ser_write_buf[MB2P] = magic_start2;
|
||||
|
||||
Serial.begin(BAUDRATE);
|
||||
Serial.setTimeout(0);
|
||||
|
||||
resetComm();
|
||||
|
||||
curState = WAITING_FOR_NEW_COM;
|
||||
|
||||
}
|
||||
|
||||
int sendCommand(int16_t len)
|
||||
{
|
||||
if((len + net_extra_len) > buf_size)
|
||||
return -1;
|
||||
|
||||
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));
|
||||
|
||||
}
|
||||
|
||||
int sendData(int len, uint8_t *buf){
|
||||
|
||||
if((len + net_extra_len) > buf_size)
|
||||
return -1;
|
||||
|
||||
memcpy(ser_write_buf + preamble_len, buf, len);
|
||||
|
||||
return sendCommand(len);
|
||||
|
||||
}
|
||||
|
||||
int runInpCommand() {
|
||||
|
||||
int data_len = com_len;
|
||||
|
||||
switch(com_recv[COMB]) {
|
||||
|
||||
case GET_BAUDRATE:
|
||||
|
||||
if(com_len != GET_BAUDRATE_LEN)
|
||||
{
|
||||
data_len = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
memcpy(ser_write_buf + DATASTRBP, &BAUDRATE, 4);
|
||||
|
||||
data_len = 4;
|
||||
|
||||
break;
|
||||
|
||||
case GET_ENCODERS:
|
||||
{
|
||||
|
||||
if(com_len != GET_ENCODERS_LEN)
|
||||
{
|
||||
data_len = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
long l = readEncoder(LEFT);
|
||||
long r = readEncoder(RIGHT);
|
||||
|
||||
memcpy(ser_write_buf + DATASTRBP, &l, 4);
|
||||
memcpy(ser_write_buf + DATASTRBP + 4, &r, 4);
|
||||
|
||||
data_len = 8;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case RESET_ENCODERS:
|
||||
|
||||
if(com_len != RESET_ENCODERS_LEN)
|
||||
{
|
||||
data_len = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
resetEncoders();
|
||||
resetPID();
|
||||
|
||||
data_len = 0;
|
||||
|
||||
break;
|
||||
|
||||
case SET_SPEEDS:
|
||||
|
||||
if(com_len != SET_SPEEDS_LEN)
|
||||
{
|
||||
data_len = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
data_len = 0;
|
||||
|
||||
break;
|
||||
|
||||
case SET_PID_L:
|
||||
|
||||
if(com_len != SET_PID_LEN)
|
||||
{
|
||||
data_len = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
setPID_L(com_recv + DATASTRB);
|
||||
|
||||
data_len = 0;
|
||||
|
||||
break;
|
||||
|
||||
case SET_PID_R:
|
||||
|
||||
if(com_len != SET_PID_LEN)
|
||||
{
|
||||
data_len = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
setPID_R(com_recv + DATASTRB);
|
||||
|
||||
data_len = 0;
|
||||
|
||||
break;
|
||||
|
||||
case GET_PID_P:
|
||||
|
||||
if(com_len != GET_PID_P_LEN)
|
||||
{
|
||||
data_len = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
getPID(ser_write_buf + DATASTRBP);
|
||||
|
||||
data_len = 8;
|
||||
|
||||
break;
|
||||
|
||||
case GET_BATTERY_LVL:
|
||||
|
||||
if(com_len != GET_BATTERY_LVL_LEN)
|
||||
{
|
||||
data_len = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
getBattLvl(ser_write_buf + DATASTRBP);
|
||||
|
||||
data_len = 2;
|
||||
|
||||
break;
|
||||
|
||||
case EN_IMU:
|
||||
|
||||
if(com_len != EN_IMU_LEN)
|
||||
{
|
||||
data_len = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
enable_imu(com_recv[DATASTRB]);
|
||||
|
||||
ser_write_buf[DATASTRBP] = 1;
|
||||
data_len = 1;
|
||||
|
||||
break;
|
||||
|
||||
case EN_ODOM:
|
||||
|
||||
if(com_len != EN_ODOM_LEN)
|
||||
{
|
||||
data_len = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
enable_odom(com_recv[DATASTRB]);
|
||||
|
||||
ser_write_buf[DATASTRBP] = 1;
|
||||
data_len = 1;
|
||||
|
||||
break;
|
||||
|
||||
|
||||
default:
|
||||
|
||||
ser_write_buf[DATASTRBP] = com_recv[COMB];
|
||||
com_recv[COMB] = INVALID;
|
||||
data_len = 1;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
if(data_len == -1)
|
||||
{
|
||||
ser_write_buf[COMBP] = INVALID_LENGTH;
|
||||
ser_write_buf[CIDBP] = com_recv[CIDB];
|
||||
ser_write_buf[DATASTRBP] = com_recv[COMB];
|
||||
data_len = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
ser_write_buf[COMBP] = com_recv[COMB];
|
||||
ser_write_buf[CIDBP] = com_recv[CIDB];
|
||||
}
|
||||
|
||||
sendCommand(data_len + com_preamble_len);
|
||||
|
||||
}
|
||||
|
||||
void resetComm(){
|
||||
com_len = 1;
|
||||
in_buf_i = 0;
|
||||
}
|
||||
|
||||
uint8_t temp_buf[64];
|
||||
void serCommTimeout(){
|
||||
Serial.readBytes(temp_buf, 64);
|
||||
}
|
||||
|
||||
void proc_serial(){
|
||||
|
||||
if(!Serial.available())
|
||||
return;
|
||||
|
||||
move_cmd_timeout_reset();
|
||||
|
||||
switch(curState){
|
||||
|
||||
case WAITING_FOR_NEW_COM:
|
||||
|
||||
if(Serial.read() == magic_start1)
|
||||
{
|
||||
curState = FIRST_BYTE_VERIFIED;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case FIRST_BYTE_VERIFIED:
|
||||
|
||||
if(Serial.read() == magic_start2)
|
||||
{
|
||||
curState = SECOND_BYTE_VERIFIED;
|
||||
}
|
||||
else
|
||||
{
|
||||
curState = WAITING_FOR_NEW_COM;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case SECOND_BYTE_VERIFIED:
|
||||
|
||||
com_len = Serial.read();
|
||||
curState = LENGTH_INPUT;
|
||||
|
||||
break;
|
||||
|
||||
case LENGTH_INPUT:
|
||||
|
||||
com_len = com_len | (Serial.read() << 8);
|
||||
|
||||
if(com_len < 20)
|
||||
{
|
||||
curState = READING_PAYLOAD;
|
||||
}
|
||||
else
|
||||
{
|
||||
curState = WAITING_FOR_NEW_COM;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case READING_PAYLOAD:
|
||||
|
||||
in_buf_i += Serial.readBytes(&in_buf[in_buf_i], com_len - in_buf_i);
|
||||
|
||||
if(in_buf_i == com_len)
|
||||
{
|
||||
curState = READING_COMPLETE;
|
||||
}
|
||||
break;
|
||||
|
||||
case READING_COMPLETE:
|
||||
|
||||
if(Serial.read() == magic_end){
|
||||
|
||||
memcpy(com_recv, in_buf, com_len);
|
||||
|
||||
runInpCommand();
|
||||
|
||||
curState = WAITING_FOR_NEW_COM;
|
||||
}
|
||||
|
||||
resetComm();
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
if((Serial.available() > 0) && curState != WAITING_FOR_NEW_COM)
|
||||
proc_serial();
|
||||
}
|
||||
12
src/libraries/Arduino/comm.h
Normal file
12
src/libraries/Arduino/comm.h
Normal file
@ -0,0 +1,12 @@
|
||||
#ifndef COMM_H
|
||||
#define COMM_H
|
||||
|
||||
const unsigned long BAUDRATE = 115200;
|
||||
|
||||
extern int init_comm();
|
||||
extern void proc_serial();
|
||||
extern int sendData(int, uint8_t *buf);
|
||||
extern void serCommTimeout();
|
||||
extern void resetComm();
|
||||
|
||||
#endif
|
||||
@ -1,16 +1,35 @@
|
||||
#ifndef COMMANDS_H
|
||||
#define COMMANDS_H
|
||||
|
||||
#define GET_BAUDRATE 'b'
|
||||
#define READ_ENCODERS 'e'
|
||||
#define MOTOR_SPEEDS 'm'
|
||||
#define RESET_ENCODERS 'r'
|
||||
#define UPDATE_PID_L 'L'
|
||||
#define UPDATE_PID_R 'R'
|
||||
#define DISP_PID_P 'z'
|
||||
#define BATTERY_LVL 'B'
|
||||
#define LEFT 0
|
||||
#define RIGHT 1
|
||||
#define INVALID 0xFF
|
||||
#define INVALID_LENGTH 0xFE
|
||||
|
||||
#define GET_BAUDRATE 0x51
|
||||
#define GET_ENCODERS 0x52
|
||||
#define SET_SPEEDS 0x53
|
||||
#define RESET_ENCODERS 0x54
|
||||
#define SET_PID_L 0x55
|
||||
#define SET_PID_R 0x56
|
||||
#define GET_PID_P 0x57
|
||||
#define GET_BATTERY_LVL 0x58
|
||||
#define EN_IMU 0x59
|
||||
#define EN_ODOM 0x60
|
||||
#define ODOM_DATA 0x61
|
||||
#define IMU_DATA 0x62
|
||||
|
||||
#define GET_BAUDRATE_LEN 0x01
|
||||
#define GET_ENCODERS_LEN 0x01
|
||||
#define SET_SPEEDS_LEN 0x03
|
||||
#define RESET_ENCODERS_LEN 0x01
|
||||
#define SET_PID_LEN 0x05
|
||||
#define GET_PID_P_LEN 0x01
|
||||
#define GET_BATTERY_LVL_LEN 0x01
|
||||
#define EN_IMU_LEN 0x02
|
||||
#define EN_ODOM_LEN 0x02
|
||||
#define ODOM_DATA_LEN 0x08
|
||||
#define IMU_DATA_LEN 0x08
|
||||
|
||||
#define LEFT 0x00
|
||||
#define RIGHT 0x01
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
160
src/libraries/Arduino/diff_controller.cpp
Normal file
160
src/libraries/Arduino/diff_controller.cpp
Normal file
@ -0,0 +1,160 @@
|
||||
#include "qt_pi.h"
|
||||
|
||||
typedef struct {
|
||||
|
||||
double TargetTicksPerFrame;
|
||||
long Encoder;
|
||||
long PrevEnc;
|
||||
|
||||
int PrevInput;
|
||||
int ITerm;
|
||||
|
||||
uint8_t Kp = 45;
|
||||
uint8_t Kd = 0;
|
||||
uint8_t Ki = 5;
|
||||
uint8_t Ko = 3;
|
||||
|
||||
uint8_t base_pwm = 70;
|
||||
|
||||
long output;
|
||||
|
||||
}
|
||||
|
||||
SetPointInfo;
|
||||
|
||||
SetPointInfo leftPID, rightPID;
|
||||
|
||||
unsigned char moving = 0;
|
||||
|
||||
void resetPID(){
|
||||
|
||||
leftPID.TargetTicksPerFrame = 0.0;
|
||||
leftPID.Encoder = readEncoder(LEFT);
|
||||
leftPID.PrevEnc = leftPID.Encoder;
|
||||
leftPID.output = 0;
|
||||
leftPID.PrevInput = 0;
|
||||
leftPID.ITerm = 0;
|
||||
|
||||
rightPID.TargetTicksPerFrame = 0.0;
|
||||
rightPID.Encoder = readEncoder(RIGHT);
|
||||
rightPID.PrevEnc = rightPID.Encoder;
|
||||
rightPID.output = 0;
|
||||
rightPID.PrevInput = 0;
|
||||
rightPID.ITerm = 0;
|
||||
|
||||
}
|
||||
|
||||
void doPID(SetPointInfo * p) {
|
||||
|
||||
long Perror;
|
||||
long output;
|
||||
int input;
|
||||
|
||||
input = p->Encoder - p->PrevEnc;
|
||||
Perror = p->TargetTicksPerFrame - input;
|
||||
|
||||
p->ITerm += (p->Ki) * Perror;
|
||||
output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
|
||||
|
||||
if(p->TargetTicksPerFrame > 0)
|
||||
{
|
||||
output += p->base_pwm;
|
||||
}
|
||||
else if(p->TargetTicksPerFrame < 0)
|
||||
{
|
||||
output -= p->base_pwm;
|
||||
}
|
||||
|
||||
p->PrevEnc = p->Encoder;
|
||||
|
||||
if (output >= MAX_PWM)
|
||||
{
|
||||
output = MAX_PWM;
|
||||
}
|
||||
else if (output <= -MAX_PWM)
|
||||
{
|
||||
output = -MAX_PWM;
|
||||
}
|
||||
else
|
||||
{
|
||||
p->output = output;
|
||||
}
|
||||
|
||||
p->PrevInput = input;
|
||||
}
|
||||
|
||||
void updatePID() {
|
||||
|
||||
leftPID.Encoder = readEncoder(LEFT);
|
||||
rightPID.Encoder = readEncoder(RIGHT);
|
||||
|
||||
if (!moving){
|
||||
|
||||
if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0)
|
||||
resetPID();
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if(leftPID.TargetTicksPerFrame != 0)
|
||||
{
|
||||
doPID(&leftPID);
|
||||
}
|
||||
|
||||
if(rightPID.TargetTicksPerFrame != 0)
|
||||
{
|
||||
doPID(&rightPID);
|
||||
}
|
||||
|
||||
setMotorSpeeds(leftPID.output, rightPID.output);
|
||||
}
|
||||
|
||||
void setPID_L(uint8_t *buf){
|
||||
|
||||
leftPID.Kp = buf[0];
|
||||
leftPID.Kd = buf[1];
|
||||
leftPID.Ki = buf[2];
|
||||
leftPID.Ko = buf[3];
|
||||
|
||||
}
|
||||
|
||||
void setPID_R(uint8_t *buf){
|
||||
|
||||
rightPID.Kp = buf[0];
|
||||
rightPID.Kd = buf[1];
|
||||
rightPID.Ki = buf[2];
|
||||
rightPID.Ko = buf[3];
|
||||
}
|
||||
|
||||
void moveTimeout(){
|
||||
|
||||
setMotorSpeeds(0, 0);
|
||||
moving = 0;
|
||||
resetPID();
|
||||
|
||||
}
|
||||
|
||||
void setMoving(bool t){
|
||||
moving = t;
|
||||
}
|
||||
|
||||
void getPID(uint8_t *buf){
|
||||
|
||||
buf[0] = leftPID.Kp;
|
||||
buf[1] = leftPID.Kd;
|
||||
buf[2] = leftPID.Ki;
|
||||
buf[3] = leftPID.Ko;
|
||||
|
||||
buf[4] = rightPID.Kp;
|
||||
buf[5] = rightPID.Kd;
|
||||
buf[6] = rightPID.Ki;
|
||||
buf[7] = rightPID.Ko;
|
||||
|
||||
}
|
||||
|
||||
void setTicksPerFrame(uint8_t *buf){
|
||||
|
||||
leftPID.TargetTicksPerFrame = buf[0];
|
||||
rightPID.TargetTicksPerFrame = buf[1];
|
||||
|
||||
}
|
||||
@ -1,79 +1,26 @@
|
||||
typedef struct {
|
||||
double TargetTicksPerFrame;
|
||||
long Encoder;
|
||||
long PrevEnc;
|
||||
#ifndef DIFF
|
||||
#define DIFF
|
||||
|
||||
int PrevInput;
|
||||
#define PID_RATE 30
|
||||
#define AUTO_STOP_INTERVAL 2000
|
||||
|
||||
int ITerm;
|
||||
int Kp = 45;
|
||||
int Kd = 0;
|
||||
int Ki = 5;
|
||||
int Ko = 3;
|
||||
int base_pwm = 70;
|
||||
long output;
|
||||
}
|
||||
const int PID_INTERVAL = 1000 / PID_RATE;
|
||||
|
||||
SetPointInfo;
|
||||
const int MAX_PWM = 255;
|
||||
|
||||
SetPointInfo leftPID, rightPID;
|
||||
extern void resetPID();
|
||||
|
||||
extern void updatePID();
|
||||
|
||||
extern void moveTimeout();
|
||||
|
||||
extern void setPID_R(uint8_t *);
|
||||
extern void setPID_L(uint8_t *);
|
||||
extern void getPID(uint8_t *);
|
||||
|
||||
extern void setTicksPerFrame(uint8_t *);
|
||||
|
||||
extern void setMoving(bool);
|
||||
|
||||
|
||||
unsigned char moving = 0;
|
||||
|
||||
void resetPID(){
|
||||
leftPID.TargetTicksPerFrame = 0.0;
|
||||
leftPID.Encoder = readEncoder(LEFT);
|
||||
leftPID.PrevEnc = leftPID.Encoder;
|
||||
leftPID.output = 0;
|
||||
leftPID.PrevInput = 0;
|
||||
leftPID.ITerm = 0;
|
||||
|
||||
rightPID.TargetTicksPerFrame = 0.0;
|
||||
rightPID.Encoder = readEncoder(RIGHT);
|
||||
rightPID.PrevEnc = rightPID.Encoder;
|
||||
rightPID.output = 0;
|
||||
rightPID.PrevInput = 0;
|
||||
rightPID.ITerm = 0;
|
||||
}
|
||||
|
||||
void doPID(SetPointInfo * p) {
|
||||
long Perror;
|
||||
long output;
|
||||
int input;
|
||||
|
||||
input = p->Encoder - p->PrevEnc;
|
||||
Perror = p->TargetTicksPerFrame - input;
|
||||
p->ITerm += (p->Ki) * Perror;
|
||||
output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
|
||||
|
||||
if(p->TargetTicksPerFrame > 0) output += p->base_pwm;
|
||||
else if(p->TargetTicksPerFrame < 0) output -= p->base_pwm;
|
||||
|
||||
p->PrevEnc = p->Encoder;
|
||||
|
||||
if (output >= MAX_PWM)
|
||||
output = MAX_PWM;
|
||||
else if (output <= -MAX_PWM)
|
||||
output = -MAX_PWM;
|
||||
else
|
||||
|
||||
p->output = output;
|
||||
p->PrevInput = input;
|
||||
}
|
||||
|
||||
void updatePID() {
|
||||
|
||||
leftPID.Encoder = readEncoder(LEFT);
|
||||
rightPID.Encoder = readEncoder(RIGHT);
|
||||
|
||||
if (!moving){
|
||||
if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
|
||||
return;
|
||||
}
|
||||
|
||||
if(leftPID.TargetTicksPerFrame != 0) doPID(&leftPID);
|
||||
if(rightPID.TargetTicksPerFrame != 0) doPID(&rightPID);
|
||||
|
||||
setMotorSpeeds(leftPID.output, rightPID.output);
|
||||
}
|
||||
#endif
|
||||
|
||||
93
src/libraries/Arduino/encoder_driver.cpp
Normal file
93
src/libraries/Arduino/encoder_driver.cpp
Normal file
@ -0,0 +1,93 @@
|
||||
#include "qt_pi.h"
|
||||
|
||||
volatile long left_enc_pos = 0L;
|
||||
volatile long right_enc_pos = 0L;
|
||||
static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};
|
||||
|
||||
ISR (PCINT1_vect){
|
||||
|
||||
static uint8_t enc_last=0;
|
||||
|
||||
enc_last <<= 2;
|
||||
enc_last |= (PINC & (3 << 2)) >> 2;
|
||||
|
||||
left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
|
||||
|
||||
}
|
||||
|
||||
ISR (PCINT2_vect){
|
||||
|
||||
static uint8_t enc_last=0;
|
||||
|
||||
enc_last <<=2;
|
||||
enc_last |= (PIND & (3 << 2)) >> 2;
|
||||
|
||||
right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
|
||||
|
||||
}
|
||||
|
||||
void init_enc(){
|
||||
|
||||
DDRC &= ~(1<<LEFT_ENC_PIN_A);
|
||||
DDRC &= ~(1<<LEFT_ENC_PIN_B);
|
||||
DDRD &= ~(1<<RIGHT_ENC_PIN_A);
|
||||
DDRD &= ~(1<<RIGHT_ENC_PIN_B);
|
||||
|
||||
//enable pull up resistors
|
||||
PORTC |= (1<<LEFT_ENC_PIN_A);
|
||||
PORTC |= (1<<LEFT_ENC_PIN_B);
|
||||
PORTD |= (1<<RIGHT_ENC_PIN_A);
|
||||
PORTD |= (1<<RIGHT_ENC_PIN_B);
|
||||
|
||||
// tell pin change mask to listen to left encoder pins
|
||||
PCMSK1 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
|
||||
// tell pin change mask to listen to right encoder pins
|
||||
PCMSK2 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
|
||||
|
||||
// enable PCINT1 and PCINT2 interrupt in the general interrupt mask
|
||||
PCICR |= (1 << PCIE1) | (1 << PCIE2);
|
||||
|
||||
}
|
||||
|
||||
long readEncoder(int i) {
|
||||
|
||||
if (i == LEFT)
|
||||
return left_enc_pos;
|
||||
|
||||
else
|
||||
return right_enc_pos;
|
||||
|
||||
}
|
||||
|
||||
void resetEncoder(int i) {
|
||||
|
||||
if (i == LEFT){
|
||||
left_enc_pos=0L;
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
right_enc_pos=0L;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void resetEncoders() {
|
||||
|
||||
resetEncoder(LEFT);
|
||||
resetEncoder(RIGHT);
|
||||
|
||||
}
|
||||
|
||||
void sendOdom(){
|
||||
|
||||
static uint8_t buf[8 + com_preamble_len];
|
||||
|
||||
buf[COMB] = ODOM_DATA;
|
||||
buf[CIDB] = 0xFF;
|
||||
|
||||
memcpy(buf + DATASTRB, &left_enc_pos, 4);
|
||||
memcpy(buf + DATASTRB + 4, &right_enc_pos, 4);
|
||||
|
||||
sendData(buf, 8 + com_preamble_len);
|
||||
}
|
||||
@ -1,10 +1,19 @@
|
||||
#ifndef ENCODER_H
|
||||
#define ENCODER_H
|
||||
|
||||
#define LEFT_ENC_PIN_A PC2
|
||||
#define LEFT_ENC_PIN_B PC3
|
||||
|
||||
#define RIGHT_ENC_PIN_A PD2
|
||||
#define RIGHT_ENC_PIN_B PD3
|
||||
|
||||
long readEncoder(int i);
|
||||
void resetEncoder(int i);
|
||||
void resetEncoders();
|
||||
#define ODOM_RATE 50
|
||||
const int ODOM_INTERVAL = 1000 / ODOM_RATE;
|
||||
|
||||
extern void init_enc();
|
||||
extern void resetEncoder(int);
|
||||
extern void resetEncoders();
|
||||
extern long readEncoder(int);
|
||||
extern void sendOdom();
|
||||
|
||||
#endif
|
||||
|
||||
@ -1,43 +0,0 @@
|
||||
volatile long left_enc_pos = 0L;
|
||||
volatile long right_enc_pos = 0L;
|
||||
static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};
|
||||
|
||||
ISR (PCINT1_vect){
|
||||
static uint8_t enc_last=0;
|
||||
|
||||
enc_last <<=2;
|
||||
enc_last |= (PINC & (3 << 2)) >> 2;
|
||||
|
||||
left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
|
||||
}
|
||||
|
||||
ISR (PCINT2_vect){
|
||||
static uint8_t enc_last=0;
|
||||
|
||||
enc_last <<=2;
|
||||
enc_last |= (PIND & (3 << 2)) >> 2;
|
||||
|
||||
right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
|
||||
}
|
||||
|
||||
long readEncoder(int i) {
|
||||
if (i == LEFT) return left_enc_pos;
|
||||
else return right_enc_pos;
|
||||
}
|
||||
|
||||
void resetEncoder(int i) {
|
||||
if (i == LEFT){
|
||||
left_enc_pos=0L;
|
||||
return;
|
||||
} else {
|
||||
right_enc_pos=0L;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void resetEncoders() {
|
||||
resetEncoder(LEFT);
|
||||
resetEncoder(RIGHT);
|
||||
}
|
||||
|
||||
|
||||
43
src/libraries/Arduino/imu.cpp
Normal file
43
src/libraries/Arduino/imu.cpp
Normal file
@ -0,0 +1,43 @@
|
||||
#include "qt_pi.h"
|
||||
|
||||
void initIMU(){
|
||||
|
||||
Wire.begin();
|
||||
|
||||
uint8_t buf;
|
||||
|
||||
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);
|
||||
*/
|
||||
}
|
||||
|
||||
void sendIMU() {
|
||||
|
||||
static uint8_t buf[14 + com_preamble_len];
|
||||
|
||||
I2Cdev::readBytes(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, 14, buf + DATASTRB);
|
||||
|
||||
buf[COMB] = IMU_DATA;
|
||||
buf[CIDB] = BID;
|
||||
|
||||
sendData(14 + com_preamble_len, buf);
|
||||
|
||||
}
|
||||
36
src/libraries/Arduino/imu.h
Normal file
36
src/libraries/Arduino/imu.h
Normal file
@ -0,0 +1,36 @@
|
||||
#ifndef IMU_H
|
||||
#define IMU_H
|
||||
|
||||
#include "I2Cdev.h"
|
||||
#include "Wire.h"
|
||||
|
||||
#define MPU6050_DEFAULT_ADDRESS 0x68
|
||||
#define MPU6050_RA_PWR_MGMT_1 0x6B
|
||||
#define MPU6050_PWR1_CLKSEL_BIT 2
|
||||
#define MPU6050_PWR1_CLKSEL_LENGTH 3
|
||||
#define MPU6050_CLOCK_PLL_XGYRO 0x01
|
||||
#define MPU6050_RA_GYRO_CONFIG 0x1B
|
||||
#define MPU6050_RA_ACCEL_CONFIG 0x1C
|
||||
#define MPU6050_GCONFIG_FS_SEL_BIT 4
|
||||
#define MPU6050_GCONFIG_FS_SEL_LENGTH 2
|
||||
#define MPU6050_ACONFIG_AFS_SEL_BIT 4
|
||||
#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2
|
||||
#define MPU6050_GYRO_FS_250 0x00
|
||||
|
||||
#define MPU6050_ACCEL_FS_2 0x00
|
||||
#define MPU6050_RA_PWR_MGMT_1 0x6B
|
||||
#define MPU6050_PWR1_SLEEP_BIT 6
|
||||
#define MPU6050_WHO_AM_I_BIT 6
|
||||
#define MPU6050_WHO_AM_I_LENGTH 6
|
||||
#define MPU6050_RA_WHO_AM_I 0x75
|
||||
#define MPU6050_RA_ACCEL_XOUT_H 0x3B
|
||||
|
||||
#define IMU_RATE 100
|
||||
|
||||
const int IMU_INTERVAL = 1000 / IMU_RATE;
|
||||
|
||||
extern void initIMU();
|
||||
|
||||
extern void sendIMU();
|
||||
|
||||
#endif
|
||||
72
src/libraries/Arduino/motor_driver.cpp
Normal file
72
src/libraries/Arduino/motor_driver.cpp
Normal file
@ -0,0 +1,72 @@
|
||||
#include "qt_pi.h"
|
||||
|
||||
void initMotorController() {
|
||||
|
||||
digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
|
||||
digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
|
||||
|
||||
}
|
||||
|
||||
void setMotorSpeed(int i, int spd) {
|
||||
|
||||
unsigned char reverse = 0;
|
||||
|
||||
if (spd < 0)
|
||||
{
|
||||
spd = -spd;
|
||||
reverse = 1;
|
||||
}
|
||||
|
||||
if (spd > 255)
|
||||
{
|
||||
spd = 255;
|
||||
}
|
||||
|
||||
if(spd == 0)
|
||||
{
|
||||
digitalWrite(LEFT_MOTOR_FORWARD, 1);
|
||||
digitalWrite(LEFT_MOTOR_BACKWARD, 1);
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
switch(i){
|
||||
|
||||
case LEFT:
|
||||
|
||||
switch(reverse)
|
||||
{
|
||||
case 0:
|
||||
analogWrite(LEFT_MOTOR_FORWARD, spd);
|
||||
analogWrite(LEFT_MOTOR_BACKWARD, 0);
|
||||
break;
|
||||
case 1:
|
||||
analogWrite(LEFT_MOTOR_FORWARD, 0);
|
||||
analogWrite(LEFT_MOTOR_BACKWARD, spd);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case RIGHT:
|
||||
switch(reverse)
|
||||
{
|
||||
case 0:
|
||||
analogWrite(RIGHT_MOTOR_FORWARD, spd);
|
||||
analogWrite(RIGHT_MOTOR_BACKWARD, 0);
|
||||
break;
|
||||
case 1:
|
||||
analogWrite(RIGHT_MOTOR_FORWARD, 0);
|
||||
analogWrite(RIGHT_MOTOR_BACKWARD, spd);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
|
||||
|
||||
setMotorSpeed(LEFT, leftSpeed);
|
||||
setMotorSpeed(RIGHT, rightSpeed);
|
||||
|
||||
}
|
||||
@ -1,3 +1,6 @@
|
||||
#ifndef MOTOR_H
|
||||
#define MOTOR_H
|
||||
|
||||
#define RIGHT_MOTOR_BACKWARD 5
|
||||
#define LEFT_MOTOR_BACKWARD 6
|
||||
#define RIGHT_MOTOR_FORWARD 9
|
||||
@ -5,6 +8,10 @@
|
||||
#define RIGHT_MOTOR_ENABLE 12
|
||||
#define LEFT_MOTOR_ENABLE 13
|
||||
|
||||
void initMotorController();
|
||||
void setMotorSpeed(int i, int spd);
|
||||
void setMotorSpeeds(int leftSpeed, int rightSpeed);
|
||||
extern void initMotorController();
|
||||
|
||||
extern void setMotorSpeed(int, int);
|
||||
|
||||
extern void setMotorSpeeds(int, int);
|
||||
|
||||
#endif
|
||||
|
||||
@ -1,32 +0,0 @@
|
||||
void initMotorController() {
|
||||
digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
|
||||
digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
|
||||
}
|
||||
|
||||
void setMotorSpeed(int i, int spd) {
|
||||
unsigned char reverse = 0;
|
||||
|
||||
if (spd < 0)
|
||||
{
|
||||
spd = -spd;
|
||||
reverse = 1;
|
||||
}
|
||||
if (spd > 255)
|
||||
spd = 255;
|
||||
|
||||
if (i == LEFT) {
|
||||
if(spd == 0) { digitalWrite(LEFT_MOTOR_FORWARD, 1); digitalWrite(LEFT_MOTOR_BACKWARD, 1); }
|
||||
else if (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); }
|
||||
else if (reverse == 1) { analogWrite(LEFT_MOTOR_BACKWARD, spd); analogWrite(LEFT_MOTOR_FORWARD, 0); }
|
||||
}
|
||||
else if (i == RIGHT){
|
||||
if(spd == 0) { digitalWrite(RIGHT_MOTOR_FORWARD, 1); digitalWrite(RIGHT_MOTOR_BACKWARD, 1); }
|
||||
else if (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); }
|
||||
else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); }
|
||||
}
|
||||
}
|
||||
|
||||
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
|
||||
setMotorSpeed(LEFT, leftSpeed);
|
||||
setMotorSpeed(RIGHT, rightSpeed);
|
||||
}
|
||||
27
src/libraries/Arduino/payload_specs.h
Normal file
27
src/libraries/Arduino/payload_specs.h
Normal file
@ -0,0 +1,27 @@
|
||||
#ifndef PAYLOAD_SPECS_H
|
||||
#define PAYLOAD_SPECS_H
|
||||
|
||||
#define MB1P 0
|
||||
#define MB2P 1
|
||||
#define LB1P 2
|
||||
#define LB2P 3
|
||||
#define CIDBP 4
|
||||
#define COMBP 5
|
||||
#define DATASTRBP 6
|
||||
|
||||
#define start_magic_len 2
|
||||
#define end_magic_len 1
|
||||
#define len_len 2
|
||||
|
||||
#define BID 0xFF
|
||||
|
||||
#define CIDB 0
|
||||
#define COMB 1
|
||||
#define DATASTRB 2
|
||||
|
||||
const int com_preamble_len = 2;
|
||||
|
||||
const int preamble_len = start_magic_len + len_len;
|
||||
const int net_extra_len = preamble_len + end_magic_len;
|
||||
|
||||
#endif
|
||||
20
src/libraries/Arduino/qt_pi.h
Normal file
20
src/libraries/Arduino/qt_pi.h
Normal file
@ -0,0 +1,20 @@
|
||||
#ifndef QT_PI_H
|
||||
#define QT_PI_H
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "commands.h"
|
||||
#include "payload_specs.h"
|
||||
|
||||
#include "imu.h"
|
||||
#include "comm.h"
|
||||
#include "batt.h"
|
||||
#include "motor_driver.h"
|
||||
#include "encoder_driver.h"
|
||||
#include "diff_controller.h"
|
||||
|
||||
extern void move_cmd_timeout_reset();
|
||||
extern void enable_imu(bool);
|
||||
extern void enable_odom(bool);
|
||||
|
||||
#endif
|
||||
253
src/serial_driver.cpp
Normal file
253
src/serial_driver.cpp
Normal file
@ -0,0 +1,253 @@
|
||||
#include <qt_pi/serial_driver.h>
|
||||
|
||||
int open_port(const char *port) {
|
||||
int fd;
|
||||
|
||||
fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
|
||||
if (fd == -1) {
|
||||
perror("open_port: Unable to open - ");
|
||||
} else
|
||||
fcntl(fd, F_SETFL, FNDELAY);
|
||||
|
||||
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 &= ~HUPCL;
|
||||
options.c_cflag |= CS8;
|
||||
options.c_cc[VTIME] = 100;
|
||||
options.c_cc[VMIN] = 1;
|
||||
|
||||
if (tcsetattr(fd, TCSANOW, &options)) {
|
||||
perror("Setting options failed.\n");
|
||||
close(fd);
|
||||
}
|
||||
|
||||
return (fd);
|
||||
}
|
||||
|
||||
volatile int sig = 1;
|
||||
|
||||
void runRecvCom(uint16_t com_len, unsigned char *com) {
|
||||
|
||||
switch (com[COMB]) {
|
||||
|
||||
case GET_BAUDRATE:
|
||||
|
||||
unsigned long baud;
|
||||
memcpy(&baud, com + DATASTRB, 4);
|
||||
|
||||
BAUDRATE_CB(com[CIDB], baud);
|
||||
|
||||
break;
|
||||
|
||||
case GET_ENCODERS: {
|
||||
|
||||
long l, r;
|
||||
|
||||
memcpy(&l, com + DATASTRB, 4);
|
||||
memcpy(&r, com + DATASTRB + 4, 4);
|
||||
|
||||
ENCODERS_CB(com[CIDB], l, r);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case GET_PID_P:
|
||||
|
||||
PID_P_CB(com[CIDB], com + DATASTRB);
|
||||
|
||||
break;
|
||||
|
||||
case GET_BATTERY_LVL:
|
||||
|
||||
uint16_t batt_lvl;
|
||||
memcpy(&batt_lvl, com + DATASTRB, 2);
|
||||
|
||||
BATT_LVL_CB(com[CIDB], batt_lvl);
|
||||
|
||||
break;
|
||||
|
||||
case IMU_DATA:
|
||||
|
||||
IMU_DATA_CB(com[CIDB], com + DATASTRB);
|
||||
|
||||
break;
|
||||
|
||||
case ODOM_DATA:
|
||||
|
||||
long l, r;
|
||||
|
||||
memcpy(&l, com + DATASTRB, 4);
|
||||
memcpy(&r, com + DATASTRB + 4, 4);
|
||||
|
||||
ODOM_DATA_CB(com[CIDB], l, r);
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void *serialDataIn(void *vargp) {
|
||||
|
||||
int *fd = (int *)vargp;
|
||||
|
||||
const uint8_t magic_start1 = 0xEE;
|
||||
const uint8_t magic_start2 = 0xAE;
|
||||
const uint8_t magic_end = 0xFC;
|
||||
|
||||
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;
|
||||
|
||||
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);
|
||||
|
||||
runRecvCom(com_len, com_recv);
|
||||
}
|
||||
|
||||
curState = WAITING_FOR_NEW_COM;
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void hex2bin(const char *in, size_t len, unsigned char *out) {
|
||||
|
||||
static const unsigned char TBL[] = {
|
||||
0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 58, 59, 60, 61,
|
||||
62, 63, 64, 10, 11, 12, 13, 14, 15, 71, 72, 73, 74, 75,
|
||||
76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89,
|
||||
90, 91, 92, 93, 94, 95, 96, 10, 11, 12, 13, 14, 15};
|
||||
|
||||
static const unsigned char *LOOKUP = TBL - 48;
|
||||
|
||||
const char *end = in + len;
|
||||
|
||||
while (in < end)
|
||||
*(out++) = LOOKUP[*(in++)] << 4 | LOOKUP[*(in++)];
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
int l = 0;
|
||||
int fd = open_port(argv[1]);
|
||||
if (fd < 0)
|
||||
return 0;
|
||||
usleep(2000000);
|
||||
pthread_t thread_id;
|
||||
pthread_create(&thread_id, NULL, tRead, (void *)&fd);
|
||||
|
||||
unsigned char buffer[25];
|
||||
unsigned char in_string[50];
|
||||
|
||||
while (sig) {
|
||||
fgets(in_string, 50, stdin);
|
||||
if (in_string[0] == 'q') {
|
||||
sig = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
l = strlen(in_string) / 2;
|
||||
|
||||
hex2bin(in_string, 2 * l, buffer);
|
||||
|
||||
int n = write(fd, buffer, l);
|
||||
printf("%d written\n", n);
|
||||
for (int i = 0; i < n; i++)
|
||||
printf("%x ", buffer[i]);
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
pthread_join(thread_id, NULL);
|
||||
// int res = read(fd, buffer, 25);
|
||||
|
||||
// if(res > 0)
|
||||
// for(int i = 0; i < res; i++)
|
||||
// printf("%x ", buffer[i]);
|
||||
// printf("\n");
|
||||
close(fd);
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user