113 lines
2.6 KiB
C++
113 lines
2.6 KiB
C++
#include <Servo.h>
|
|
#include <Arduino_JSON.h>
|
|
#include <Arduino_AVRSTL.h>
|
|
#include <assert.h>
|
|
#include <map>
|
|
#include <String>
|
|
|
|
constexpr short svo_num = 2;
|
|
constexpr short svo_dig_pin[svo_num] = {9, 10};
|
|
Servo svo[svo_num]; // al, ar, tl, tr
|
|
short svo_angle[svo_num];
|
|
std::map<String, short> svo_name2idx = {{"al", 0}, {"ar", 1}};
|
|
|
|
constexpr short eng_num = 1;
|
|
constexpr short eng_dig_pin[eng_num] = {11};
|
|
Servo eng[eng_num]; // al, ar, tl, tr
|
|
short eng_speed[eng_num];
|
|
std::map<String, short> eng_name2idx = {{"e1", 0}};
|
|
JSONVar data;
|
|
|
|
void svo_set(short idx, short angle){
|
|
svo[idx].write(angle);
|
|
svo_angle[idx] = angle;
|
|
delay(15);
|
|
}
|
|
|
|
short svo_get(short idx){
|
|
return svo_angle[idx];
|
|
}
|
|
|
|
void svo_pnt_by_name(String name){
|
|
Serial.print("{\"" + name + "\":");
|
|
Serial.print(svo_angle[svo_name2idx[name]]);
|
|
Serial.print("}\r\n");
|
|
}
|
|
|
|
void svo_set_by_name(String name, short num){
|
|
svo_set(svo_name2idx[name], num);
|
|
}
|
|
|
|
void eng_set(short idx, short speed){
|
|
eng[idx].writeMicroseconds(speed);
|
|
eng_speed[idx] = speed;
|
|
delay(15);
|
|
}
|
|
|
|
short eng_get(short idx){
|
|
return eng_speed[idx];
|
|
}
|
|
|
|
void eng_pnt_by_name(String name){
|
|
Serial.print("{\"" + name + "\":");
|
|
Serial.print(eng_speed[eng_name2idx[name]]);
|
|
Serial.print("}\r\n");
|
|
}
|
|
|
|
void eng_set_by_name(String name, short num){
|
|
eng_set(eng_name2idx[name], num);
|
|
}
|
|
|
|
void setup() {
|
|
for(short i = 0; i < svo_num; ++i){
|
|
svo[i].attach(svo_dig_pin[i]);
|
|
}
|
|
for(short i = 0; i < svo_num; ++i){
|
|
svo_set(i, 0);
|
|
}
|
|
for(short i = 0; i < eng_num; ++i){
|
|
eng[i].attach(eng_dig_pin[i]);
|
|
}
|
|
for(short i = 0; i < eng_num; ++i){
|
|
eng_set(i, 0);
|
|
}
|
|
Serial.begin(9600);
|
|
//Serial.println("\r\n");
|
|
}
|
|
void handle(String type, String device, String option, short num){
|
|
if(type == "svo"){
|
|
if(option == "q"){
|
|
svo_pnt_by_name(device);
|
|
}
|
|
if(option == "s"){
|
|
svo_set_by_name(device, num);
|
|
}
|
|
}
|
|
if(type == "eng"){
|
|
if(option == "q"){
|
|
eng_pnt_by_name(device);
|
|
}
|
|
if(option == "s"){
|
|
eng_set_by_name(device, num);
|
|
}
|
|
}
|
|
}
|
|
|
|
void loop() {
|
|
//Serial.print("hihihi\r\n");
|
|
//Serial.print("\r\n");
|
|
while(Serial.available() == 0);
|
|
data = JSON.parse(Serial.readString());
|
|
if(JSON.typeof(data) == "undefined") {
|
|
Serial.print("json_failed\r\n");
|
|
return;
|
|
}
|
|
handle(data["type"], data["device"], data["option"], data["num"]);
|
|
/*
|
|
data["t(ype)"]: svo/eng
|
|
data["d(evice)"]: al, ar, tl, tr
|
|
data["o(ption)"]: s(et)/q(uery)
|
|
data["n(um)"]: SHORT
|
|
*/
|
|
}
|