Orange Pi 5에서 모터와 기타 단순 관리용 서브모듈로 사용하는 Arduino Nano와의 시리얼 통신용 코드.
이 방식은 아래와 같이 형태의 4글자 명령어와 최대 4개의 파라메터를 함께 보낼 수 있는 방식이다. 기체의 두뇌인 Orange Pi 5에서 온갖 명령어를 보내서 단순 작업들을 아두이노 나노에게 실행시키는 방법이다.
<ESPC:3,4,-29334,4>
<ESCT:0>
Parses.h
#define BUFFER_SIZE 50
int P2B_NEWLINE = 11; // < -- Beginning of Command to Remote Control that controls R/C Car
int P2B_COM_TYPE = 12; // 0..9 -- First 2 letters following "<"
// Total Command Types : 10 x 10 = 100
int P2B_DATA = 12; // Data section
int P2B_ENDING = 13; // > -- End of Message Signal
int P2B_STAT_NONE = 1; // Do nothing until '<' has received
int P2B_STAT_CMD = 2; // Accepting Command
int P2B_STAT_DATA = 3; // Accepting Command data
int P2B_STAT_END = 4; // '>' has receieve, it is the end of command;
// PI to Nano command string
char p2b_command[5];
char p2b_data[BUFFER_SIZE];
int p2b_paramcount = 0;
int p2b_status = 0;
int i = 0; // dumb int
int j = 0; // dumb int
int k = 0; // dumb int
int n = 0; // dumb int
int lp =0; // dumb last position of input command string
int r = 0; // Simple Counter to replace delay() in the main loop
String s; // Temporary string for general purpose
String rs;
String cmd; // used on ExecuteLogicalCommand()
boolean debugSerial = true;
boolean ContionousMove = true;
char inputString[200]; // a string to hold incoming data
boolean stringComplete = false; // whether the string is complete
char inChar;
String tempStr;
char buffer[12]; // buffer used for itoa (int to string conversion)
int inlen = 0; // Length of inputString
char RemoteString[50];
// value from PC Command (integer params 1 .. 4 max)
int paramcount = 0; // How many params in input String
int param1 = -1; // value from PC Command (Param 1)
int param2 = -1; // value from PC Command (Param 2)
int param3 = -1; // value from PC Command (Param 3)
int param4 = -1; // value from PC Command (Param 4)
int tempVal = 0;
int nPos = 0;
int sign = 1; // 1 = plus, -1 = minus
float fparam1 = 0.0;
float fparam2 = 0.0;
float fparam3 = 0.0;
float fparam4 = 0.0;
boolean SecondParam = false; // Now gettign second parameter (after ,)
// NEW PINS 18 = cam
const int CAMPin = 5;// the number of the LED pin
int freq = 5000;
int resolution = 8;
int dist = 0;
int StartingSpeed = 0; // 0 - 255 (8 bit)
int targetHeight = 0;
int msAi = 0;
int msBi = 0;
int msCi = 0;
int msDi = 0;
int maxSpeed = 50;
int height = 0;
int targetLR = 0; // Target Left/Right
int targetFB = 0; // Target Forward / Backward
int targetTN = 0; // Target Turn (+ CW, - CCW)
boolean mStop = false; // MS, Stop Motors
boolean mGoInAngle = false; // MG, Set Motor Speeds and Directions
boolean contGetAll = false; //
boolean AutoBalancing = false;
boolean NoOffset = false;
////////////////////////////////////////////////////////////////
void ClearCommand() {
p2b_command[0] = 0; // null
for (int i=0; i <= 49; i++){
p2b_data[i] = 0; // null
}
// set as none, 0 could be value
param1 = -1; //
param2 = -1; //
param3 = -1; //
param4 = -1; //
SecondParam = false;
}
void ClearRemoteString() {
for (i=0;i<=49;i++) {
RemoteString[i] = 0;
};
}
void ClearParamsAndInputstring(){
// clear the string:
for (i=0;i<=49;i++) {
inputString[i] = 0;
};
i = 0;
j = 0;
paramcount = 0; // How many params in input String
param1 = -1;
param2 = -1;
param3 = -1;
param4 = -1;
fparam1 = 0.0;
fparam2 = 0.0;
fparam3 = 0.0;
fparam4 = 0.0;
stringComplete = false;
}
MAR01AR.ino
/* This is written by Henry Kim, as a M.A.R. (Machine of Attack & Return)
Currently testing level @ Oct. 1, 2023
3 ESC for Propellers
- Center Tail Propeller
- Left Wing Propeller
- Right Wing Propeller
3 Servo 메인 모터 180도 회전
- for 180 degree rotation of 3 Propeller Motors
10 LED
1 A/D Battery Level Monitor
1 A/D Light Sensor
3 A/D Sound Sensor
1 A/D Temperature Sensor
*/
#include <Servo.h>
#include "Parses.h"
// ---------------------------------------------------------------------------
// Customize here pulse lengths as needed
#define MIN_PULSE_LENGTH 1000 // Minimum pulse length in µs (1 ms : min speed)
#define MAX_PULSE_LENGTH 2000 // Maximum pulse length in µs (2 ms : max speed)
// ---------------------------------------------------------------------------
Servo escLeft1, escRight2, escCenter3;
Servo motLeft4, motRight5, motCenter6;
// ---------------------------------------------------------------------------
void setup() {
ClearCommand();
Serial.begin(115200);
Serial.println("#MAR-NANO>");
// 3 main ESC for Props
escLeft1.attach(4, MIN_PULSE_LENGTH, MAX_PULSE_LENGTH);
escRight2.attach(5, MIN_PULSE_LENGTH, MAX_PULSE_LENGTH);
escCenter3.attach(6, MIN_PULSE_LENGTH, MAX_PULSE_LENGTH);
// Servos - Change angles of Prop motors
motLeft4.attach(7);
motRight5.attach(8);
motCenter6.attach(9);
// Prevent motor run at the start
stopAllProps(); // set in MIN_PULSE_LENGTH
// Servos heading top initially
basePosition4Servos(); // Heading Top
}
void loop() {
r++; //
if (r > 300000) { r = 0; } // Reset r if it is too large
if ((r % 20000) == 0) { // Do some sensor reading
//
}
while (Serial.available()) {
inChar = (char)Serial.read();
// get the new byte:
//Serial.print(inChar); // for debut
// add it to the inputString:
inputString[i] = inChar;
i++;
// if the incoming character is a newline, set a flag
if (inChar == '<') {
p2b_status = P2B_STAT_CMD;
ClearCommand();
j = i-1;
} else if (inChar == '>') { // so the main loop can do something about it:
p2b_command[0] = inputString[j+1]; // Get Command Character
p2b_command[1] = inputString[j+2]; // Get Command Character
p2b_command[2] = inputString[j+3]; // Get Command Character
p2b_command[3] = inputString[j+4]; // Get Command Character
// O : Orientation
// D : Distance
p2b_paramcount = int(inputString[j+6])-48; // take out 0
j = j + 8; // now j is pointing param starting position
if (p2b_paramcount > 0) {
tempVal = 0;
nPos = 0;
sign = 1; // 1 = plus, -1 = minus
k = 1;
for (n=j;n<=lp+1;n++){
// end of param
if ((inputString[n] == ',') or (inputString[n] == '>')){
if (k == 1){
param1 = tempVal * sign;
}
if (k == 2){
param2 = tempVal * sign;
}
if (k == 3){
param3 = tempVal * sign;
}
if (k == 4){
param4 = tempVal * sign;
}
tempVal = 0;
k++; // Param No.
nPos = 0;
sign = 1; // 1 = plus, -1 = minus
} else {
if (inputString[n] == '-'){
sign = -1;
} else {
nPos++;
if (nPos == 1) {
tempVal = (byte(inputString[n]) - 48);
} else {
tempVal = tempVal * 10 + (byte(inputString[n]) - 48);
}
}
}
}
} // end of if command is for this Machine
Serial.println("ESCT TEST 00");
ExecuteLogicalCommand();
i = 100; // refresh counter
stringComplete = true;
p2b_status = P2B_STAT_NONE;
} else {
lp = i-1; // set last position on each entry;
}
} /* while (Serial.available()) { */
if (stringComplete) {
uPrint(); // Return for debug
ClearParamsAndInputstring();
}
}
void ExecuteLogicalCommand() {
cmd = (char)p2b_command[0]+(char)p2b_command[1]+(char)p2b_command[2]+(char)p2b_command[3];
if (cmd == "ESCT"){ // Test ESC for Prop Motors
testESC();
};
if (cmd == "SRVT"){ // Test All Servos
testServos();
}
}
void basePosition4Servos(){
// Servos heading top initially
motLeft4.write(180); // Heading Top
motRight5.write(180); // Heading Top
motCenter6.write(180); // Heading Top
}
void testServos() // 3 Servos : escLeft1, escRight2, escCenter3
{
// Heading top to bottom
for (int i = 0; i <= 180; i += 10) {
motLeft4.write(180-i);
motRight5.write(180-i);
motCenter6.write(180-i);
delay(100);
// Heading bottom to top
for (int i = 0; i <= 180; i += 10) {
motLeft4.write(i);
motRight5.write(i);
motCenter6.write(i);
delay(100);
}
basePosition4Servos();
}
void stopAllProps() {
escLeft1.writeMicroseconds(MIN_PULSE_LENGTH);
escRight2.writeMicroseconds(MIN_PULSE_LENGTH);
escCenter3.writeMicroseconds(MIN_PULSE_LENGTH);
}
void testESC() // 3 ESC Test : escLeft1, escRight2, escCenter3
{
for (int i = MIN_PULSE_LENGTH; i <= MAX_PULSE_LENGTH-700; i += 40) {
// Serial.print("Pulse length = ");
// Serial.println(i);
escLeft1.writeMicroseconds(i);
escRight2.writeMicroseconds(i);
escCenter3.writeMicroseconds(i);
delay(200);
}
stopAllProps();
}
