基于双核NXP-MCXN947的避障巡线车

/*******************************************************************************
* Copyright ? 2023 Analog Devices Inc. All Rights Reserved.
* This software is proprietary to Analog Devices, Inc. and its licensors.
*******************************************************************************/

#include "Arduino.h"
#include <60ghzbreathheart.h>
#include <SPI.h>
#include <afstandssensor.h>
#include <Servo.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 afstandssensor1(A2, A3);
AfstandsSensor afstandssensor2(A4, A5);

BreathHeart_60GHz radar = BreathHeart_60GHz(&Serial1);
Servo directservo;

#define AVGVELOCITY 0x00002710
#define SLOWVELOCITY 0x00001000
#define STOPVELOCITY 0x00000000

#define LEFTDIRECT 15
#define RIGHTDIRECT -15

#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 pos = 0;
//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;
int Velocity= 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 = afstandssensor1.afstandCM();
rightSonic = afstandssensor2.afstandCM();
Velocity = SLOWVELOCITY;

if (leftSonic+ rightSonic < 10 ){
leftVelocity = STOPVELOCITY;
rightVelocity = STOPVELOCITY;
Velocity = AVGVELOCITY;
pos=0;
//directservo.write(pos);
} // Stop rover

/****************** Quick Reaction
if (leftSonic < 20 and rightSonic < 20){
leftVelocity = AVGVELOCITY;
rightVelocity = STOPVELOCITY;
Velocity = SLOWVELOCITY;
pos=0;
//directservo.write(pos);
} // Turn around rover
**********************/

if (leftSonic > 20 and rightSonic < 20){
leftVelocity = STOPVELOCITY;
rightVelocity = SLOWVELOCITY;
Velocity = SLOWVELOCITY;
pos=LEFTDIRECT;
//directservo.write(pos);
} // Turn rover left

if (leftSonic < 20 and rightSonic > 20){
leftVelocity = SLOWVELOCITY;
rightVelocity = STOPVELOCITY;
Velocity = SLOWVELOCITY;
pos=RIGHTDIRECT;
//directservo.write(pos);
} // Turn rover right

directservo.write(pos);
delay(1000);
tmc5272_rotateMotor(IC_ID, 0, Velocity);
//tmc5272_rotateMotor(IC_ID, 1, rightVelocity);

}

void humanFollowing(void){
float radarDirection= 0;
leftSonic = afstandssensor1.afstandCM();
rightSonic = afstandssensor2.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, Velocity);
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);
}

pinMode(2, INPUT_PULLUP); //Get Human Detection from MCXN947
directservo.attach(3);
delay(10);

//digitalWrite(iRefR2, HIGH);
//digitalWrite(iRefR3, HIGH);
Serial.println(afstandssensor1.afstandCM());
Serial.println(afstandssensor2.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();
//STOP if HUMAN detected
if ( digitalRead(2)==HIGH ) {
tmc5272_rotateMotor(IC_ID, 0, 0x00000000);
}

}

?