현재 상태 : PC와의 UPD 통신, PWM, Motor Controller 용 GPIO, Encoder Reading 완료.
추가할 내용 : Timer Interrupt, I2C for GY-BNO055, PID
#include <WiFi.h>
#include <WiFiUdp.h>
#include <Wire.h>
#include "Parse.h"
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
// ESP32's I2C Pin No.
#define _ESP32_HAL_I2C_H_
#ifdef _ESP32_HAL_I2C_H_
#define SDA_PIN 32
#define SCL_PIN 12
#endif
#define BUFFER_SIZE 100
#define THIS_MACHINE "002" // Set this bot's no. : 2
const int ThisMachine = 2;
//IP address to send UDP data to:
const char * udpAddress = "192.168.1.100"; // 저의 PC의 고정주소
const int udpPort = 6767; // 제가 로봇용으로 주로 사용하는 포트번호
const char serverip[]="192.168.1.100";
char packetBuffer[255];
//The udp library class
WiFiUDP udp;
//Are we currently connected?
boolean connected = false;
float ax, ay, az, aSqrt, gx, gy, gz, mDirection, mx, my, mz;
const char * host = "ESP32-02";
const char * networkName = "****"; // 집이나 사무실 와이파이 아이디
const char * networkPswd = "****"; // 와이파이 연결 비밀번호
char botresp[50] = "001-CCCC:4,000,000,000,000"; // default init size should be size of string + 1
String ToServerStr;
char ReplyBuffer[] = "acknowledged"; // a string to send back
unsigned long preMillis = 0;
unsigned long curMillis = 0;
/////////// BNO-055
TwoWire I2CBNO = TwoWire(0);
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x29, &I2CBNO);
double xPos = 0, yPos = 0, headingVel = 0;
uint16_t BNO055_SAMPLERATE_DELAY_MS = 10; //how often to read data from the board
void ExecuteLogicalCommand() {
// WHON : Wheel StandBy : Params ( on / off )
if (((char)p2b_command[0] == 'W') and ((char)p2b_command[1] == 'H')) {
if (debugSerial) {
s = "<*WHON="; s += p2b_paramcount; s += ">";
Serial.println(s);
}
digitalWrite(StandBy, param1);
reset_encoder_count();
};
// FWDM : Forward Move 양 바퀴 전진, 첫 파라메터는 좌측 바퀴 속도, 둘째는 우측 바퀴 속도.
if (((char)p2b_command[0] == 'F') and ((char)p2b_command[1] == 'W')) {
if (debugSerial) {
s = "<*FWDM="; s += p2b_paramcount; s += ">";
Serial.println(s);
}
reset_encoder_count();
// Left Wheel (A)
digitalWrite(Ain1, 1);
digitalWrite(Ain2, 0);
ledcWrite(WheelChannelLeft, param1);
// Right Wheel (B)
digitalWrite(Bin1, 0);
digitalWrite(Bin2, 1);
ledcWrite(WheelChannelRight, param2);
};
// BWDM : Backward Move 양 바퀴 후진, 첫 파라메터는 좌측 바퀴 속도, 둘째는 우측 바퀴 속도.
if (((char)p2b_command[0] == 'B') and ((char)p2b_command[1] == 'W')) {
if (debugSerial) {
s = "<*BWDM="; s += p2b_paramcount; s += ">";
Serial.println(s);
}
reset_encoder_count();
// Left Wheel (A)
digitalWrite(Ain1, 0);
digitalWrite(Ain2, 1);
ledcWrite(WheelChannelLeft, param1);
// Right Wheel (B) // Currently one signal not working on B (33, 34)
digitalWrite(Bin1, 1);
digitalWrite(Bin2, 0);
ledcWrite(WheelChannelRight, param2);
};
}
void setupControlPins(){
freq = 5000;
resolution = 8;
StartingSpeed = 0; // 0 - 255 (8 bit)
ledcSetup(WheelChannelLeft, freq, resolution);
ledcSetup(WheelChannelRight, freq, resolution);
ledcAttachPin(WheelLeft, WheelChannelLeft);
ledcAttachPin(WheelRight, WheelChannelRight);
ledcWrite(WheelRight, StartingSpeed);
pinMode(Ain2, OUTPUT);
pinMode(Ain1, OUTPUT);
pinMode(StandBy, OUTPUT);
pinMode(Bin1, OUTPUT);
pinMode(Bin2, OUTPUT);
ledcWrite(WheelLeft, StartingSpeed);
// Two encoder pins for wheels
pinMode(Encoder2_A_Left, INPUT);
pinMode(Encoder2_B_Right, INPUT);
}
void reset_encoder_count() { // Reset before start moving
count_AL = 0;
count_BR = 0;
last_count_AL = 0;
last_count_BR = 0;
}
void ISR_Left_Encoder() {
count_AL++;
}
void ISR_Right_Encoder() {
count_BR++;
}
void setup() {
setupControlPins(); // IO 핀들 세팅할 것.
// 모든 시리얼은 드론에는 사용 못하고 다른 ESP32 Development Board에서 개발/디버깅 중에만 사용 가능합니다.
Serial.begin(115200); // 시리얼을 안쓰더라도 반드시 이렇게 놔 둘 것.
//Connect to the WiFi network 네트웍에 연결
connectToWiFi(networkName, networkPswd);
// Wait for connection
while (WiFi.status() != WL_CONNECTED) {
delay(500);
Serial.print(".");
}
// 모든 시리얼은 무선 작동시에는 사용 못하고 디버깅 중에만 사용 가능합니다.
/* Wire.begin(SDA_PIN, SCL_PIN); // ESP32는 반드시 이렇게.
//Serial.println("I2C enabled! ");
I2CBNO.begin(SDA_PIN, SCL_PIN);
if (!bno.begin()){
Serial.print("No BNO055 detected");
while (1);
} */
delay(1000);
r = 0;
// Encoder Setting
pinMode(Encoder2_A_Left, INPUT_PULLUP);
attachInterrupt(Encoder2_A_Left, ISR_Left_Encoder, FALLING);
pinMode(Encoder2_B_Right, INPUT_PULLUP);
attachInterrupt(Encoder2_B_Right, ISR_Right_Encoder, FALLING);
}
void uPrint(){ // print whatever in "udpStr" with station # and ending ">"
s = "#"; s += THIS_MACHINE; s += ":"; s += udpStr; s += ">";
udp.beginPacket(udpAddress,udpPort);
udp.print(s);
udp.endPacket();
}
void loop() {
r++;
if ((r % 3000) == 0){
if(connected){
//Send a packet
udpStr = "#L/R: "; udpStr += count_AL; udpStr += ", "; udpStr += count_BR; ;
uPrint(); // it prints whatever in udpString
}
}
// Expected message format : <002-MECH:4,10,10,70,70>
// "<" beginning mark, 002 mechine no., MECH command, 4 params, + 4 values ,">" ending mark
// 3 digit machine no., 4 char command, # of params, params seperated by ","
int packetSize = udp.parsePacket();
if (packetSize) {
int len = udp.read(packetBuffer, 255); // here, len = packetSize
if (len > 0) {
packetBuffer[len] = 0;
if (len < 51) {
for (i=1;i<51;i++)
botresp[i+3] = packetBuffer[i];
}
};
i = 0;
while ((packetSize) >= i){
inChar = packetBuffer[i]; // get the new byte:
inputString[i] = inChar;
i++;
// if the incoming character is a newline, set a flag
if ((inChar == '#') or (inChar == '<')) {
p2b_status = P2B_STAT_CMD;
ClearP2BCommand();
j = i-1;
// ===== Debug Purpose =====
Serial.println("시작점 #, <");
} else if (inChar == '>') { // so the main loop can do something about it:
// First, get the machine # first, who is this command to
p2b_machno = 0;
if (inputString[j+1] != '0') {
p2b_machno = (int(inputString[j+1])-48) * 100;
}
if (inputString[j+2] != '0') {
p2b_machno = p2b_machno + (int(inputString[j+2])-48) * 10;
}
if (inputString[j+3] != '0') {
p2b_machno = p2b_machno + (int(inputString[j+3])-48);
}
// If this command is for ME (This machine)
if (ThisMachine == p2b_machno){
p2b_mach[0] = inputString[j+1];
p2b_mach[1] = inputString[j+2];
p2b_mach[2] = inputString[j+3];
p2b_command[0] = inputString[j+5];
p2b_command[1] = inputString[j+6];
p2b_command[2] = inputString[j+7];
p2b_command[3] = inputString[j+8];
p2b_paramcount = int(inputString[j+10])-48; // take out 0
Serial.print("Machine : ");
Serial.println(p2b_machno);
Serial.print("Command :");
Serial.println(p2b_command);
Serial.print("Parameter Count : ");
Serial.println(p2b_paramcount);
j = j + 12; // now j is pointing param starting position
if (p2b_paramcount > 0) {
tempVal = 0;
nPos = 0;
k = 1;
for (n=j;n<=packetSize;n++){
// end of param
if ((inputString[n] == ',') or (inputString[n] == '>')){
if (k == 1){
param1 = tempVal;
Serial.print("Param 1 : ");
Serial.println(param1);
}
if (k == 2){
param2 = tempVal;
Serial.print("Param 2 : ");
Serial.println(param2);
}
if (k == 3){
param3 = tempVal;
Serial.print("Param 3 : ");
Serial.println(param3);
}
if (k == 4){
param4 = tempVal;
Serial.print("Param 4 : ");
Serial.println(param4);
}
tempVal = 0;
k++; // Param No.
nPos = 0;
} else {
if (nPos == 1) {
tempVal = (byte(inputString[n]) - 48);
} else {
tempVal = tempVal * 10 + (byte(inputString[n]) - 48);
}
}
}
}
// Serial.println("");
ExecuteLogicalCommand();
} // end of if command is for this Machine
i = 100; // refresh counter
stringComplete = true;
p2b_status = P2B_STAT_NONE;
} else {
lp = i-1; // set last position on each entry;
}
}
} // while ((packetSize) >= i){
if (stringComplete) {
ClearParamsAndInputstring();
};
if (r == 5000) {
// if (contGetAll) { GetAll(); }
// if (contAccel) { Get3DAccel(); }
// if (contGyro) { Get3DGyro(); }
// if (contMag) { Get3DMag(); }
// if (contHeight) { GetHeight(); }
}
// Reset r
if (r > 1000000) { r = 0; }
}
void connectToWiFi(const char * ssid, const char * pwd){
Serial.println("Connecting to WiFi network: " + String(ssid));
// delete old config
WiFi.disconnect(true);
//register event handler
WiFi.onEvent(WiFiEvent);
//Initiate connection
WiFi.begin(ssid, pwd);
connected = true;
Serial.println("Waiting for WIFI connection...");
}
//wifi event handler
void WiFiEvent(WiFiEvent_t event){
switch(event) {
case SYSTEM_EVENT_STA_GOT_IP:
//When connected set
Serial.print("WiFi connected! IP address: ");
Serial.println(WiFi.localIP());
//initializes the UDP state
//This initializes the transfer buffer
udp.begin(WiFi.localIP(),udpPort);
connected = true;
break;
case SYSTEM_EVENT_STA_DISCONNECTED:
Serial.println("WiFi lost connection");
connected = false;
break;
default: break;
}
}
Parse.h 화일
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;
char p2b_mach[4];
char p2b_command[5];
char p2b_data[50];
int p2b_machno = 0;
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 udpStr;
boolean debugSerial = false;
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 = 1;
float fparam1 = 0.0;
float fparam2 = 0.0;
float fparam3 = 0.0;
float fparam4 = 0.0;
boolean SecondParam = false; // Now gettign second parameter (after ,)
// Wheel PWM Related Definitions
// A = Left, B = Right
int WheelChannelLeft = 0; // motor A PWM Channel
int WheelChannelRight = 1; // motor B PWM Channel
// NEW PINS 19, 23, 26, 5, 18
const int WheelLeft = 19; // GREY - motor A on my design sheet
const int Ain2 = 25; // WHITE
const int Ain1 = 23; // ORANGE
const int StandBy = 5; // YELLOW
const int Bin1 = 14; // PINK (34 didn't work, changed to 14
const int Bin2 = 33; // BROWN
const int WheelRight = 18; // BLUE - motor B on my design sheet
// Speed Read from Encoder
const int Encoder2_A_Left = 36; // Yellow Color
const int Encoder2_B_Right = 26; // Yellow Color
int count_AL = 0;
int count_BR = 0;
int last_count_AL = 0;
int last_count_BR = 0;
int freq = 5000;
int resolution = 8;
int StartingSpeed = 0; // 0 - 255 (8 bit)
int targetHeight = 0;
int msAi = 0;
int msBi = 0;
int msCi = 0;
int msDi = 0;
int maximumSpeed = 50;
int height = 0;
int targetLR = 0; // Target Left/Right
int targetFB = 0; // Target Forward / Backward
int targetTN = 0; // Target Turn (+ CW, - CCW)
boolean contMag = false;
boolean contAccel = false;
boolean contGyro = false;
boolean contHeight = false;
boolean contGetAll = false;
boolean AutoBalancing = true;
boolean NoOffset = false;
////////////////////////////////////////////////////////////////
void ClearP2BCommand() {
p2b_mach[0] = 0; // null
p2b_mach[1] = 0; // null
p2b_mach[2] = 0; // null
p2b_mach[3] = 0; // null
p2b_command[0] = 0; // null
p2b_command[1] = 0; // null
p2b_command[2] = 0; // null
p2b_command[3] = 0; // null
p2b_command[4] = 0; // null
for (int i=0; i <= 49; i++){
p2b_data[i] = 0; // null
}
p2b_machno = 0;
// 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;
}'ESP32 BOT > My ESP32 Bot' 카테고리의 다른 글
| My ESP32 Bot - The beginning. (0) | 2021.11.03 |
|---|
