June 06, 2018 by The Tactigon
Learn how to control a simple tracked rover using Euler angles. The remote controller can be put in a glove!
We'll look at how to control a simple tracked rover using Euler's angles. The Remote controller can be put in a glove and become wearable!
We love robots, and we love wearables. Why not build a wearable rover controller?
We used two The Tactigon boards, as they are programmable with Arduino IDE and offer Bluetooth low energy radio, 9DoF IMU and low power consumption.
The target was to control our tracked rover using a board as transmitter, which uses euler's angles to determine tracks speed, and another board as receiver, which communicates with motor control board.
As shown above, The Tactigon is connected to the motor control board (form factor may differs). Anyway you can use an Arduino UNO for this task. The Tactigon is used here for further expansions of this project.
This video shows the rover controlled with a smartphone.
The Tactigon used as remote controller does the math to determine tracks speed. The sketch attached shows how to handle pitch and roll.
#include <tactigon_led.h>
#include <tactigon_BLE.h>
#include <tactigon_UserSerial.h>
//RGB LEDs
T_Led rLed, bLed, gLed;
//BLE Manager, BLE Characteristic and its UUID
T_BLE bleManager;
T_BLE_Characteristic cmdChar;
UUID uuid;
//UART
T_UserSerial tSerial;
//Used for LEDs cycle
int ticksLed, stp;
/*----------------------------------------------------------------------*/
void cbBLEcharWritten(uint8_t *pData, uint8_t dataLen)
{
blinkLEDs();
cingoSpeed((char*)pData, dataLen);
}
/*----------------------------------------------------------------------*/
void blinkLEDs() {
//Blinks LEDs, turned off by next state in loop()
rLed.on();
gLed.on();
bLed.on();
}
/*----------------------------------------------------------------------*/
void cingoSpeed(char *spd, uint8_t len) {
tSerial.write(spd, len);
}
/*----------------------------------------------------------------------*/
void setup() {
char charProg;
ticksLed = 0;
stp = 0;
rLed.init(T_Led::RED);
gLed.init(T_Led::GREEN);
bLed.init(T_Led::BLUE);
rLed.off();
gLed.off();
bLed.off();
//init role
bleManager.InitRole(TACTIGON_BLE_PERIPHERAL);
//bleManager.setName("CINGO");
uuid.set("c1c0760d-503d-4920-b000-101e7306b001"); //command characteristic UUID
cmdChar = bleManager.addNewChar(uuid, 18, 1); //create characteristic
charProg = cmdChar.setWcb(cbBLEcharWritten); //setup a callback on characteristic write event
//init user serial
tSerial.init(T_UserSerial::B_9600, T_UserSerial::T_SERIAL1);
}
/*----------------------------------------------------------------------*/
void loop() {
//Cycle on rgb leds
if (GetCurrentMilli() >= (ticksLed + (3000 / 1))) {
ticksLed = GetCurrentMilli();
if (stp == 0) {
rLed.on();
gLed.off();
bLed.off();
}
else if (stp == 1) {
rLed.off();
gLed.off();
bLed.on();
}
else if (stp == 2) {
rLed.off();
gLed.on();
bLed.off();
}
stp = (stp + 1) % 3;
}
}#include <tactigon_led.h>
#include <tactigon_IMU.h>
#include <tactigon_BLE.h>
extern int ButtonPressed;
T_Led rLed, bLed, gLed;
T_QUAT qMeter;
T_QData qData;
T_BLE bleManager;
UUID targetUUID;
uint8_t targetMAC[6] = {0xbe, 0xa5, 0x7f, 0x2e, 0x7d, 0x4b};
T_BLE_Characteristic accChar, gyroChar, magChar, qChar;
int ticks, ticksLed, stp, cnt, printCnt, ledCnt;
float roll, pitch, yaw;
//////////////////////////////////////////////////////////////////////////////////////////
void setup() {
// put your setup code here, to run once:
ticks = 0;
ticksLed = 0;
stp = 0;
cnt = 0;
ledCnt = 0;
//init leds
rLed.init(T_Led::RED);
gLed.init(T_Led::GREEN);
bLed.init(T_Led::BLUE);
rLed.off();
gLed.off();
bLed.off();
//init BLE
bleManager.setName("Tacti");
bleManager.InitRole(TACTIGON_BLE_CENTRAL); //BLE role: CENTRAL
targetUUID.set("c1c0760d-503d-4920-b000-101e7306b001"); //target characteristic
bleManager.setTarget(targetMAC, targetUUID); //target: mac device and its char UUID
}
//////////////////////////////////////////////////////////////////////////////////////////
void loop() {
char buffData[2];
int deltaWheel, speedWheel;
int pitchThreshold, rollThreshold, th1, th2;
//base engine @ 50Hz (20msec)
if (GetCurrentMilli() >= (ticks + (1000 / 50))) {
ticks = GetCurrentMilli();
//get quaternions and Euler angles
qData = qMeter.getQs();
//Euler angles: rad/sec --> degrees/sec
roll = qData.roll * 360 / 6.28;
pitch = qData.pitch * 360 / 6.28;
yaw = qData.yaw * 360 / 6.28;
//forward/backword
rollThreshold = 10;
th1 = 90 + rollThreshold;
th2 = 90 - rollThreshold;
roll = fabs(roll);
//compute speed wheel and delta speed wheel depending on Euler angles
//left/right
pitchThreshold = 10;
if (pitch < -pitchThreshold || pitch > pitchThreshold) {
if (pitch < -pitchThreshold) {
if (roll < th1 && roll > th2) {
//spin
deltaWheel = - (fabs(pitch) - pitchThreshold) * 10;
rLed.on();
bLed.on();
gLed.on();
} else {
deltaWheel = - (fabs(pitch) - pitchThreshold) * 3;
}
}
else {
if (roll < th1 && roll > th2) {
//spin
deltaWheel = + (fabs(pitch) - pitchThreshold) * 10;
rLed.on();
bLed.on();
gLed.on();
} else {
deltaWheel = + (fabs(pitch) - pitchThreshold) * 3;
}
}
}
else {
deltaWheel = 0;
}
//forward/backward
if (roll > th1) {
speedWheel = ((roll - th1) * 3);
}
else if (roll < th2) {
speedWheel = ((roll - th2) * 3);
}
else {
speedWheel = 0;
}
//convert speedWheel and deltaWheel in command byte for motor control board
int sxC, dxC;
uint8_t sx, dx;
sxC = (speedWheel - (-deltaWheel / 4)) + 64;
dxC = (speedWheel + (-deltaWheel / 4)) + 192;
if (sxC > 127) {
sxC = 127;
} else if (sxC < 1) {
sxC = 1;
}
if (dxC < 128) {
dxC = 128;
} else if ( dxC > 255) {
dxC = 255;
}
sx = sxC;
dx = dxC;
//fill buffData to write in BLE characteristic
buffData[0] = sx;
buffData[1] = dx;
Serial.print("SX: ");
Serial.println(sx);
Serial.print("DX: ");
Serial.println(dx);
//if connected and attached to peripheral characteristic write in it
if (bleManager.getStatus() == 3) {
//signal that connection is on
bLed.on();
rLed.off();
ledCnt++;
if (ledCnt > 100) {
ledCnt = 0;
rLed.off();
bLed.off();
gLed.off();
}
//write in BLE characteristic (@ 10Hz in order to not stress control motor board)
cnt++;
if (cnt > 5) {
cnt = 0;
bleManager.writeToPeripheral((unsigned char *)buffData, 2);
rLed.on();
}
}
else
bLed.off();
}
}