Table of contents
Abstract
RangeDetect Rover avoid obstacles and change direction to find way out.
Project
1 Introduction
This is the project for detect obstacle and path-finding. This can be in-door delivery, or sweeping bot, or home surveillance.
2 Hardware
The TMC5272 is a Step/Dir Driver for Two-Phase Bipolar Stepper Motors up to 0.8 A (RMS) (1.12 A (PEAK)). Ttwo TRINAMIC Stepper Motorss are used for this Range Detect Rover.
There are two types of sensors used in range detecting. Ultrasonic sensor HC-SR04 and 60GHz mmWare radar sensor. The HC-SR04 ultrasonic sensor uses sonar to determine the distance to an object. This sensor reads from 2cm to 400cm (0.8inch to 157inch) with an accuracy of 0.3cm (0.1inches). 60GHz mmWave Sensor - Human Resting Breathing and Heartbeat Module has been updated, adding sleep monitoring function. This can detect range of 2000cm with smart algorithm with human detection.
MKR wifi 1010 shall be used as control board
3 The Rover framework
Hard wood and stacked carton box are used for the framework to hold the stepper motor and TMC5272 driver board.
4 The Program
This is software flow chart
And coding in arduino cloud app,
And this is the code
#include "Arduino.h"
#include <60ghzbreathheart.h>
#include <SPI.h>
#include <afstandssensor.h>
extern "C" {
#include "TMC5272_HW_Abstraction.h"
#include "TMC5272.h"
#include "TMC5272_Simple_Rotation.h"
}
/*
* Arduino Pins Eval Board Pins
* 51 MOSI 32 SPI1_SDI
* 50 MISO 33 SPI1_SDO
* 52 SCK 31 SPI1_SCK
* 53 SS 30 SPI1_CSN
* 14 USART3_TX 22 UART_RX
* 15 USART3_RX 21 UART_TX
* GND 23 CLK16 -> use internal
* 23 DIO 19 nSleep
* GND 2 GND
* +5V 5 +5V_USB
* 27 iRefR2 35 IREF_R2
* 29 iRefR3 36 IRREF_R3
* 31 uartMode 20 USEL
*/
//AfstandsSensor afstandssensor(7, 6);
AfstandsSensor afstandssensor0(A0, A1);
AfstandsSensor afstandssensor1(A2, A3);
BreathHeart_60GHz radar = BreathHeart_60GHz(&Serial1);
#define AVGVELOCITY 0x00002710
#define SLOWVELOCITY 0x00001000
#define STOPVELOCITY 0x00000000
#define IC_ID 0
static TMC5272BusType activeBus = IC_BUS_SPI;
static uint8_t nodeAddress = 0;
const uint8_t tmcCRCTable_Poly7Reflected[256] = {
0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B,
0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67,
0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43,
0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F,
0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B,
0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17,
0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33,
0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F,
0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B,
0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87,
0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3,
0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF,
0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB,
0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7,
0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3,
0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF,
};
//int nSleep = 23;
//int iRefR2 = 27;
//int iRefR3 = 29;
//int uartMode = 31;
int nSleep = 6;
int iRefR2 = 7;
int iRefR3 = 7;
int uartMode = 6;
float leftSonic = 0;
float rightSonic = 0;
float humanDist = 0;
int leftVelocity = 0;
int rightVelocity= 0;
uint8_t tmc5272_getNodeAddress(uint16_t icID) {
return nodeAddress;
}
TMC5272BusType tmc5272_getBusType(uint16_t icID) {
return activeBus;
}
void tmc5272_readWriteSPI(uint16_t icID, uint8_t *data, size_t dataLength) {
digitalWrite(PIN_SPI_SS, LOW);
delayMicroseconds(10);
for (uint32_t i = 0; i < dataLength; i++) {
data[i] = SPI.transfer(data[i]);
Serial.println(data[i]);
}
delayMicroseconds(10);
digitalWrite(PIN_SPI_SS, HIGH);
}
void radarRead(void)
{
radar.HumanExis_Func(); //Human existence information output
if(radar.sensor_report != 0x00){
switch(radar.sensor_report){
case NOONE:
Serial.println("Nobody here.");
Serial.println("----------------------------");
break;
case SOMEONE:
Serial.println("Someone is here.");
Serial.println("----------------------------");
break;
case NONEPSE:
Serial.println("No human activity messages.");
Serial.println("----------------------------");
break;
case STATION:
Serial.println("Someone stop");
Serial.println("----------------------------");
break;
case MOVE:
Serial.println("Someone moving");
Serial.println("----------------------------");
break;
case BODYVAL:
Serial.print("The parameters of human body signs are: ");
Serial.println(radar.bodysign_val, DEC);
Serial.println("----------------------------");
break;
case DISVAL:
Serial.print("The sensor judges the distance to the human body to be: ");
Serial.print(radar.distance, DEC);
Serial.println(" m");
Serial.println("----------------------------");
break;
case DIREVAL:
Serial.print("The sensor judges the orientation data with the human body as -- x: ");
Serial.print(radar.Dir_x);
Serial.print(" m, y: ");
Serial.print(radar.Dir_y);
Serial.print(" m, z: ");
Serial.print(radar.Dir_z);
Serial.println(" m");
Serial.println("----------------------------");
break;
}
}
}
void randomRoving(void){
leftSonic=afstandssensor0.afstandCM();
rightSonic=afstandssensor1.afstandCM();
if (leftSonic+ rightSonic < 10 ){
leftVelocity = STOPVELOCITY;
rightVelocity = STOPVELOCITY;
} // Stop rover
if (leftSonic < 20 and rightSonic < 20){
leftVelocity = AVGVELOCITY;
rightVelocity = STOPVELOCITY;
} // Turn around rover
if (leftSonic > 20 and rightSonic < 20){
leftVelocity = STOPVELOCITY;
rightVelocity = SLOWVELOCITY;
} // Turn rover left
if (leftSonic < 20 and rightSonic > 20){
leftVelocity = SLOWVELOCITY;
rightVelocity = STOPVELOCITY;
} // Turn rover right
tmc5272_rotateMotor(IC_ID, 0, leftVelocity);
tmc5272_rotateMotor(IC_ID, 1, rightVelocity);
delay(1000);
}
void humanFollowing(void){
float radarDirection= 0;
leftSonic=afstandssensor0.afstandCM();
rightSonic=afstandssensor1.afstandCM();
radar.HumanExis_Func(); //Human existence information output
if(radar.sensor_report != 0x00){
switch(radar.sensor_report){
case NOONE:
humanDist=0;
break;
case DISVAL:
Serial.print("The sensor judges the distance to the human body to be: ");
Serial.print(radar.distance, DEC);
humanDist=radar.distance;
break;
case DIREVAL:
Serial.print("The sensor judges the orientation data with the human body as -- x: ");
Serial.print(radar.Dir_x);
Serial.print(" m, y: ");
Serial.print(radar.Dir_y);
Serial.print(" m, z: ");
Serial.print(radar.Dir_z);
Serial.println(" m");
Serial.println("----------------------------");
radarDirection= radar.Dir_y;
break;
}
}
if (leftSonic+ rightSonic < 10 ){
leftVelocity = STOPVELOCITY;
rightVelocity = STOPVELOCITY;
} // Stop rover
if (leftSonic < 20 and rightSonic < 20){
leftVelocity = AVGVELOCITY;
rightVelocity = STOPVELOCITY;
} // Turn around rover
if (leftSonic > 20 and rightSonic < 20){
leftVelocity = STOPVELOCITY;
rightVelocity = SLOWVELOCITY;
} // Turn rover left
if (leftSonic < 20 and rightSonic > 20){
leftVelocity = SLOWVELOCITY;
rightVelocity = STOPVELOCITY;
} // Turn rover right
if (leftSonic > 20 and rightSonic > 20){
leftVelocity = AVGVELOCITY;
rightVelocity = AVGVELOCITY;
if (radarDirection > 0.5 ){
leftVelocity = leftVelocity + SLOWVELOCITY;
}
if (radarDirection < - 0.5 ){
rightVelocity = rightVelocity + SLOWVELOCITY;
}
} // Turn rover right
tmc5272_rotateMotor(IC_ID, 0, leftVelocity);
tmc5272_rotateMotor(IC_ID, 1, rightVelocity);
delay(1000);
}
void setup() {
Serial.begin(115200);
Serial1.begin(115200);
// put your setup code here, to run once:
pinMode(PIN_SPI_SS, OUTPUT);
pinMode(nSleep, OUTPUT);
pinMode(iRefR2, OUTPUT);
pinMode(iRefR3, OUTPUT);
pinMode(uartMode, OUTPUT);
digitalWrite(PIN_SPI_SS, HIGH);
digitalWrite(nSleep, LOW); // Disabling standby
digitalWrite(iRefR2, LOW);
digitalWrite(iRefR3, LOW);
if (activeBus == IC_BUS_SPI) {
digitalWrite(uartMode, LOW);
SPI.begin();
SPI.beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE3));
} else if (activeBus == IC_BUS_UART) {
digitalWrite(uartMode, HIGH);
//Serial3.begin(115200);
pinMode(PIN_SPI_MOSI, OUTPUT);
pinMode(PIN_SPI_SCK, OUTPUT);
digitalWrite(PIN_SPI_MOSI, LOW);
digitalWrite(PIN_SPI_SS, LOW);
digitalWrite(PIN_SPI_SCK, LOW);
}
delay(10);
//digitalWrite(iRefR2, HIGH);
//digitalWrite(iRefR3, HIGH);
Serial.println(afstandssensor0.afstandCM());
Serial.println(afstandssensor1.afstandCM());
initAllMotors(IC_ID);
tmc5272_rotateMotor(IC_ID, 0, AVGVELOCITY);
tmc5272_rotateMotor(IC_ID, 1, AVGVELOCITY);
}
void loop() {
int32_t value = tmc5272_readRegister(IC_ID, TMC5272_VMAX(0));
Serial.print("Received Data: "); Serial.println(value);
delay(200);
randomRoving();
//humanFollowing();
}
With function of randomRoving(), the rover avoid obstacles when two sensors, left one and right one can detect obstacles is different position. And further function can be implemented as human following.
Then the rover turns direction with different velocity for left or right stepper motor in the following codes
tmc5272_rotateMotor(IC_ID, 0, leftVelocity); tmc5272_rotateMotor(IC_ID, 1, rightVelocity);
The function tmc5272_rotateMotor(IC_ID, motor, Velocity) can control the motor in velocity. If different velocity is applied in two stepper motor, the rover turns directions accordingly.
5 The results
The rover can run randomly at home like idiot.
While some algorithms are applied, the home map can be generated automatically, then rove can be upgraded to run as Automatic Vehicle to carry goods from one place to another desitination.
References
RangeDetect Rover 2# Range Detector Sensors - element14 Community
RangeDetect Rover 3# Using MKR wifi 1010 as TMC5272 Control board - element14 Community
RangeDetect Rover 4# Quick-built Rover Platform - element14 Community
RangeDetect Rover 5# Coding for the Rover - element14 Community