パネルロボットのソースコードと共にパッドの操作説明を行う。
PS2パッドを使うことでアナログスティック2つと16個のボタンが使用可能になった。
アナログスティックL 車体の操作（M1、M2）
アナログスティックR 上下でサーボ１の操作、左右でサーボ２の操作
上下ボタン M1の正転、逆転
△、✕ボタン M2の正転、逆転
R1、R2ボタン サーボ1の操作
L1、L2ボタン サーボ２の操作
#include <ESP8266WiFi.h>
#include <WiFiClient.h>
#include <WiFiUDP.h>
#include <Servo.h>
#include "ps2pad.h"
// Variable setting
char WiFibuff[8];
unsigned char keyState[8];
// UDP setting
WiFiUDP UDP;
// WiFi setting
IPAddress myIP(192, 168, 4, 1);
const char *ssid = "********";
const char *password = "********";
const int channel = 2;
const int ssid_hidden = 0;
//led
const int ledPin = 16;
const int led1 = 4;
const int led2 = 5;
const int led3 = 12;
const int led4 = 2;
//Servo
Servo myservo1;
Servo myservo2;
const int SERVO1 = 14;
const int SERVO2 = 13;
int servo1_val = 90;
int servo2_val = 90;
void rcvWiFi() {
UDP.read(WiFibuff, 8);
UDP.flush();
}
void setup() {
Serial.begin(115200);
pinMode(ledPin, OUTPUT);
pinMode(led1, OUTPUT);
pinMode(led2, OUTPUT);
pinMode(led3, OUTPUT);
pinMode(led4, OUTPUT);
WiFi.mode(WIFI_AP);
WiFi.softAP(ssid, password);
WiFi.config(myIP, WiFi.gatewayIP(), WiFi.subnetMask());
UDP.begin(893); // setting UDP port
digitalWrite(ledPin, HIGH);
Serial.println("UDP.begin!");
myservo1.attach(SERVO1);
myservo2.attach(SERVO2);
myservo1.write(servo1_val);
myservo2.write(servo2_val);
}
void loop() {
//Serial.println(UDP.parsePacket());
if (UDP.parsePacket() > 0) {
rcvWiFi();
for(int i = 0; i < 8; i++){
keyState[i]= WiFibuff[i];
}
analog_move();
servo_move();
}
}
void analog_move(){
int v1 = keyState[7];
int v2 = keyState[6];
int v = 30;
int y = (v1 > 127) ? (v1 -127) : (127 - v1) * -1;
int x = (v2 > 127) ? (v2 -127) * -1 : (127 - v2);
int r1, r2, l1, l2, vect1;
r1 = r2 = l1 = l2 = vect1 = 0;
if(y > v || y < -v){
int tmp = abs(x) * abs(x) + abs(y) * abs(y);
vect1 = abs(round(sqrt(tmp)));
vect1 = (vect1 > 127) ? 127 : vect1;
if(y > v){
if(x > 0){
l2 = vect1;
r2 = abs(y);
}else{
r2 = vect1;
l2 = abs(y);
}
}else if(y < -v){
if(x > 0){
l1 = vect1;
r1 = abs(y);
}else{
r1 = vect1;
l1 = abs(y);
}
}
}else{
if(x > v){
r2 = x;
l1 = x;
}else if(x < -v){
r1 = -x;
l2 = -x;
}else{
}
}
analogWrite(led1, m_val(r1));
analogWrite(led2, m_val(r2));
analogWrite(led3, m_val(l1));
analogWrite(led4, m_val(l2));
if(PRESSED_UP){ analogWrite(led1, 1023); }
if(PRESSED_DOWN){ analogWrite(led2, 1023); }
if(PRESSED_TRIANGLE){ analogWrite(led3, 1023); }
if(PRESSED_CROSS){ analogWrite(led4, 1023); }
}
int m_val(int v){
return map(v, 0, 128, 0, 1023);
}
void servo_move(){
int v1 = keyState[5];
int v2 = keyState[4];
int v = 30;
int y = (v1 > 127) ? (v1 -127) : (127 - v1) * -1;
int x = (v2 > 127) ? (v2 -127) * -1 : (127 - v2);
if(x > v){ servo1_val = 80 - s_val(x) + 10; }else if(x < -v){ servo1_val = s_val(-x) + 90; }else{ servo1_val = 90; }
if(y > v){ servo2_val = 80 - s_val(y) + 10; }else if(y < -v){ servo2_val = s_val(-y) + 90; }else{ servo2_val = 90; }
if(PRESSED_R1){ servo1_val = 10; }
if(PRESSED_R2){ servo1_val = 170; }
if(PRESSED_L1){ servo2_val = 10; }
if(PRESSED_L2){ servo2_val = 170; }
myservo1.write(servo1_val);
myservo2.write(servo2_val);
}
int s_val(int v){
return map(v, 30, 128, 0, 80);
}
ps2pad.h
使用していないキーは記述していない。
#define PRESSED_LEFT ( !(keyState[2] & (1<<7)) )
#define PRESSED_DOWN ( !(keyState[2] & (1<<6)) )
#define PRESSED_RIGHT ( !(keyState[2] & (1<<5)) )
#define PRESSED_UP ( !(keyState[2] & (1<<4)) )
#define PRESSED_SQUARE ( !(keyState[3] & (1<<7)) )
#define PRESSED_CROSS ( !(keyState[3] & (1<<6)) )
#define PRESSED_CIRCLE ( !(keyState[3] & (1<<5)) )
#define PRESSED_TRIANGLE ( !(keyState[3] & (1<<4)) )
#define PRESSED_R1 ( !(keyState[3] & (1<<3)) )
#define PRESSED_L1 ( !(keyState[3] & (1<<2)) )
#define PRESSED_R2 ( !(keyState[3] & (1<<1)) )
#define PRESSED_L2 ( !(keyState[3] & 1 ) )