8ServoHat做万向小车底盘@HomeMadeGarbage
底盘使用了4个舵机，其中两个为360度舵机，两个为180度舵机，180舵机用来控制360舵机方向，360舵机作为轮子，此外安装两个万向轮用于支持。使用BLE将遥控器的X和Y数据发送到M5StickC上，控制舵机转动。遥控器选择的是任天堂的 Wii Nnchak，Nnchak与AtomLite的连接如下
使用ESP-NOW协议将 M5Stack ATOM Lite 接收的 Wii Nnchak 值与控制飞机的 M5StickC 进行无线通信
在 ESP 之间使用 WiFi 网络提供独特的通信方法，这里是Arduino IDE示例代码，即使第一次使用也很容易上手，ESP-NOW 使 ESP 之间的无线通信变得非常简单。
M5StackAtomLite端精简代码（主机）
#include <esp_now.h>
#include <WiFi.h>
#include "Wire.h"
uint8_t nunbuff; // array to store ESP output
int cnt = 0;
uint8_t x_dat = 0; // x Data
uint8_t y_dat = 0; // y Data
uint8_t z_dat = 0; // z&c Data
uint8_t x_axis; // z accele axis data
uint8_t y_axis; // y accele axis data
uint8_t z_axis; // z accele axis data
uint8_t z2 = 0;
uint8_t c2 = 0;
char str1; // 1st byte data
char str2; // 2nd byte data
char str3; // 3rd byte data
char str4;
int val1 = 0; // z button ON + c button ON
int val2 = 0; // data switching
int state = 0; // storage data
int old_val = 0; // Old value
esp_now_peer_info_t slave;
#define CHANNEL 3
#define PRINTSCANRESULTS 0
#define DELETEBEFOREPAIR 0
void InitESPNow() {
WiFi.disconnect();
if (esp_now_init() == ESP_OK) {
Serial.println("ESPNow Init Success");
}
else {
Serial.println("ESPNow Init Failed");
ESP.restart();
}
}
void ScanForSlave() {
int8_t scanResults = WiFi.scanNetworks();
bool slaveFound = 0;// reset on each scan
memset(&slave, 0, sizeof(slave));
Serial.println("");
if (scanResults == 0) {
Serial.println("No WiFi devices in AP Mode found");
} else {
Serial.print("Found "); Serial.print(scanResults); Serial.println(" devices ");
for (int i = 0; i < scanResults; ++i) {
String SSID = WiFi.SSID(i);
int32_t RSSI = WiFi.RSSI(i);
String BSSIDstr = WiFi.BSSIDstr(i);
delay(10);
if (SSID.indexOf("Slave") == 0) {
Serial.println("Found a Slave.");
Serial.print(i + 1); Serial.print(": "); Serial.print(SSID); Serial.print(" ["); Serial.print(BSSIDstr); Serial.print("]"); Serial.print(" ("); Serial.print(RSSI); Serial.print(")"); Serial.println("");
// Get BSSID => Mac Address of the Slave
int mac;
if ( 6 == sscanf(BSSIDstr.c_str(), "%x:%x:%x:%x:%x:%x%c",&mac, &mac, &mac, &mac, &mac, &mac ) ) {
for (int ii = 0; ii < 6; ++ii ) {
slave.peer_addr = (uint8_t) mac;
}
}
slave.channel = CHANNEL; // pick a channel
slave.encrypt = 0; // no encryption
slaveFound = 1;
break;
}
}
}
if (slaveFound) {
Serial.println("Slave Found, processing..");
} else {
Serial.println("Slave Not Found, trying again.");
}
WiFi.scanDelete();
}
bool manageSlave() {
if (slave.channel == CHANNEL) {
if (DELETEBEFOREPAIR) {
deletePeer();
}
Serial.print("Slave Status: ");
const esp_now_peer_info_t *peer = &slave;
const uint8_t *peer_addr = slave.peer_addr;
bool exists = esp_now_is_peer_exist(peer_addr);
if ( exists) {
// Slave already paired.
Serial.println("Already Paired");
return true;
} else {
// Slave not paired, attempt pair
esp_err_t addStatus = esp_now_add_peer(peer);
if (addStatus == ESP_OK) {
// Pair success
Serial.println("Pair success");
return true;
} else {
Serial.println("Not sure what happened");
return false;
}
}
} else {
Serial.println("No Slave found to process"); // No slave found to process
return false;
}
}
void deletePeer() {
const esp_now_peer_info_t *peer = &slave;
const uint8_t *peer_addr = slave.peer_addr;
esp_err_t delStatus = esp_now_del_peer(peer_addr);
}
// send data
void sendData() {
uint8_t data;
data = x_dat;
data = y_dat;
data = z2;
const uint8_t *peer_addr = slave.peer_addr;
Serial.print("Sending: "); //Serial.println(data);
esp_err_t result = esp_now_send(peer_addr, data, 3);
Serial.print("Send Status: ");
if (result == ESP_OK) {
Serial.println("Success");
} else {
Serial.println("Not sure what happened");
}
}
void nunchuck_init () { // In the case of blackNunchuck, change the address.
Wire.beginTransmission (0x52);// transmit to device 0x52
Wire.write (0x40); // sends memory address *(blackNunchuck:0xF0）
Wire.write (0x00); // sends sent a zero. *(blackNunchuck:0x55）
Wire.endTransmission ();// stop transmitting
}
void send_zero () { // In the case of blackNunchuck, change the address.
Wire.beginTransmission (0x52);// transmit to device 0x52
Wire.write (0x00); // sends one byte
Wire.endTransmission ();// stop transmitting
}
void setup() {
Serial.begin(115200);
Wire.begin (25, 21); //SDA, SCL
nunchuck_init (); // send the initilization handshake
WiFi.mode(WIFI_STA);
InitESPNow();
}
void loop() {
Wire.requestFrom (0x52, 6); // request data from nunchuck
while (Wire.available ()) {
nunbuff = nunchuk_decode_byte (Wire.read ());
cnt++;
}
int z = 0;
int c = 0;
if (cnt >= 5) {
x_dat = nunbuff;
y_dat = nunbuff;
x_axis = nunbuff ;
y_axis = nunbuff ;
z_axis = nunbuff ;
if ((nunbuff >> 0) & 1) z = 1; // The first bit of data of the 5byte (Nuncuck Z-button)
if ((nunbuff >> 1) & 1) c = 1; // The second bit of data of the 5byte (Nuncuck C-button)
if (z == 1 & c == 1)z_dat = 0; // The button has not been pressed.
if (z == 0 & c == 1)z_dat = 1; // When the Z button is pressed.
if (z == 1 & c == 0)z_dat = 2; // When the C button is pressed.
if (z == 0 & c == 0) { // When the Z and C buttons are pressed simultaneously.
val1 = 0;
} else {
val1 = 1;
}
if ((val1 == 0)&&(old_val)){ // State holding operation
state = 1 - state;
delay(10);
}
old_val = val1;
if (state == 1) {
val2 = 0; // mode switch
} else {
val2 = 1; // mode switch
}
delay(1);
}
z2 = z;
c2 = c;
cnt = 0;
send_zero (); // send the request for next bytes
delay (10);
if (slave.channel == CHANNEL) {
bool isPaired = manageSlave();
if (isPaired) {
sendData();
} else {
Serial.println("Slave pair failed!");
}
}
else {
ScanForSlave();
}
delay(100);
}
char nunchuk_decode_byte (char x) {
x = (x ^ 0x17) + 0x17;
return x;
}
M5StickC（从机）
#include <M5StickC.h>
#include "IIC_servo.h"
#include <esp_now.h>
#include <WiFi.h>
#define CHANNEL 1
int x = 0, y = 0, th2;
int thL, thR;
float th = 0.0;
int stopValueL = 90, stopValueR=90;
int speedL = 30, speedR = 30;
#define DEG2RAD 0.0174532925
byte inc = 0;
unsigned int col = 0;
// Init ESP Now with fallback
void InitESPNow() {
WiFi.disconnect();
if (esp_now_init() == ESP_OK) {
Serial.println("ESPNow Init Success");
}
else {
Serial.println("ESPNow Init Failed");
// Retry InitESPNow, add a counte and then restart?
// InitESPNow();
// or Simply Restart
ESP.restart();
}
}
// config AP SSID
void configDeviceAP() {
const char *SSID = "Slave_1";
bool result = WiFi.softAP(SSID, "Slave_1_Password", CHANNEL, 0);
if (!result) {
Serial.println("AP Config failed.");
} else {
Serial.println("AP Config Success. Broadcasting with AP: " + String(SSID));
}
}
void setup() {
Serial.begin(115200);
Serial.println("ESPNow/Basic/Slave Example");
M5.begin();
M5.Axp.ScreenBreath(10);
M5.Lcd.fillScreen(TFT_BLACK);
M5.Lcd.println(" ");
M5.Lcd.println(" Power ON");
IIC_Servo_Init(); //sda0 scl26
Servo_angle_set(6,stopValueL);
Servo_angle_set(3,stopValueR);
//Set device in AP mode to begin with
WiFi.mode(WIFI_AP);
// configure device AP mode
configDeviceAP();
// This is the mac address of the Slave in AP Mode
Serial.print("AP MAC: "); Serial.println(WiFi.softAPmacAddress());
// Init ESPNow with a fallback logic
InitESPNow();
// Once ESPNow is successfully Init, we will register for recv CB to
// get recv packer info.
esp_now_register_recv_cb(OnDataRecv);
}
// callback when data is recv from Master
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {
char macStr;
snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
mac_addr, mac_addr, mac_addr, mac_addr, mac_addr, mac_addr);
//Serial.print("Last Packet Recv from: "); Serial.println(macStr);
Serial.print(data);
Serial.print(", ");
Serial.print(data);
Serial.print(", ");
Serial.println(data);
x = int(data) - 132;
y = int(data) - 132;
if(abs(x) > 10 || abs(y) > 10){
th = atan2(y,-x) *180.0/M_PI;
if(th < 0){
th += 360;
}
fillSegment(40, 80, 0, 360, 35, TFT_BLACK);
fillSegment(40, 80, int(th)-90, 5, 35, TFT_GREEN);
if(th > 180.0){
//th2 = map(int(th - 180.0),0, 180, 180, 0);
th2 = int(th - 180.0);
thL = stopValueL + speedL;
thR = stopValueR - speedR;
}else{
//th2 = map(int(th),0, 180, 180, 0);
th2 = int(th);
thL = stopValueL - speedL;
thR = stopValueR + speedR;
}
Serial.println(th2);
Servo_angle_set(2,th2);
Servo_angle_set(7,th2);
Servo_angle_set(6,thL);
Servo_angle_set(3,thR);
}else{
th = 0;
fillSegment(40, 80, 0, 360, 35, TFT_BLACK);
Servo_angle_set(6,stopValueL);
Servo_angle_set(3,stopValueR);
}
}
void loop() {
}
// #########################################################################
// Draw circle segments
// #########################################################################
// x,y == coords of centre of circle
// start_angle = 0 - 359
// sub_angle = 0 - 360 = subtended angle
// r = radius
// colour = 16 bit colour value
int fillSegment(int x, int y, int start_angle, int sub_angle, int r, unsigned int colour)
{
// Calculate first pair of coordinates for segment start
float sx = cos((start_angle - 90) * DEG2RAD);
float sy = sin((start_angle - 90) * DEG2RAD);
uint16_t x1 = sx * r + x;
uint16_t y1 = sy * r + y;
// Draw colour blocks every inc degrees
for (int i = start_angle; i < start_angle + sub_angle; i++) {
// Calculate pair of coordinates for segment end
int x2 = cos((i + 1 - 90) * DEG2RAD) * r + x;
int y2 = sin((i + 1 - 90) * DEG2RAD) * r + y;
M5.Lcd.fillTriangle(x1, y1, x2, y2, x, y, colour);
// Copy segment end to sgement start for next segment
x1 = x2;
y1 = y2;
}
}
// #########################################################################
// Return the 16 bit colour with brightness 0-100%
// #########################################################################
unsigned int brightness(unsigned int colour, int brightness)
{
byte red = colour >> 11;
byte green = (colour & 0x7E0) >> 5;
byte blue= colour & 0x1F;
blue =(blue * brightness)/100;
green = (green * brightness)/100;
red = (red * brightness)/100;
return (red << 11) + (green << 5) + blue;
}
页:
[1]