#include <Ps3Controller.h>
/*PS3 Controllerからのデータを格納する変数*/
int v120 = 0; //1バイト目 リアルタイムでの取り込み値(BLE通信)
int vv120 = 0; //1バイト目 ボタンが押された時にこの変数に値を格納する(ボタン離したとき0値にならないように)
int ex120 = 0; //1バイト目 前回押されたボタン値を格納しておいて、次に押されたボタン値と比較する
// 変動してなかったら(=ボタンから指を離したままか押したままの時) vv120は0値でリセット
int v121 = 0; //2バイト目 リアルタイムでの取り込み値(BLE通信)
int vv121 = 0; //2バイト目 ボタンが押された時にこの変数に値を格納する
int ex121 = 0; //2バイト目 前回押されたボタン値を格納しておいて、次に押されたボタン値と比較する
// 変動してなかったら(=ボタンから指を離したままか押したままの時) vv121は0値でリセット
/* 使うピンの定義 */
#define AIN1 13 //左IN1 pwm@mode=0 DRV8835 IN/INモードで使う、phase/enableモードは空転がなく両方0の時にブレーキでモーター回らない
#define AIN2 12 //左IN2 pwm@mode=0 DRV8835
//#define BIN1 0 //右IN1 pwm@mode=0 DRV8835
//#define BIN2 4 //右IN2 pwm@mode=0 DRV8835
#define SERVO1 32 //steering servo
/* PWMチャンネルの定義 */
#define CHA 0 //AIN1 for 左motor
#define CHB 1 //AIN2 for 左motor
//#define CHC 2 //BIN1 for 右motor
//#define CHD 3 //BIN2 for 右motor
#define CHE 4 //for servo 1
/* モーター用PWMチャンネルの設定 */
const int LEDC_TIMER_BIT = 8; // PWMの範囲(8bitなら0〜255、10bitなら0〜1023)
const int LEDC_BASE_FREQ = 490; // 周波数(Hz)
const int VALUE_MAX = 255; // PWMの最大値
/* サーボ用PWMチャンネルの設定 */
const int LEDC_SERVO_TIMER_BIT = 10; // PWMの範囲(8bitなら0〜255、10bitなら0〜1023)
const int LEDC_SERVO_BASE_FREQ = 50; // 周波数(Hz)
const int VALUE_SERVO_MAX = 1023; // PWMの最大値
//モータースピード変数
int speedFL;
int speedBL;
//int speedFR;
//int speedBR;
//サーボ関連変数
// (26/1024)*20ms ≒ 0.5 ms (-90°)
// (74/1024)*20ms ≒ 1.45 ms (0°)
// (123/1024)*20ms ≒ 2.4 ms (+90°)
// 以下の値は 適宜 調整必要)
int posX = 70; // servo1 position (可変値)
int posX_ex = 70; // servo1 neutral position (中立角 固定値)
int servo_angle_min_X = 56; // servo1 minimum position(右最大振れ角の値)
int servo_angle_max_X = 84; // servo1 maximum position(左最大振れ角の値)
//セットアップ
void setup() {
Serial.begin(115200);
Ps3.begin("84:0D:8E:C1:6A:AE"); // My ESP32 Mac address
Serial.println("Ready");
//使うピンのpinModeセットアップ
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
//pinMode(BIN1, OUTPUT);
//pinMode(BIN2, OUTPUT);
pinMode(SERVO1, OUTPUT);
// PWMピンのセットアップ
ledcSetup(CHA, LEDC_BASE_FREQ, LEDC_TIMER_BIT); // ch0 490 Hz 8bit resolution
ledcSetup(CHB, LEDC_BASE_FREQ, LEDC_TIMER_BIT); // ch1 490 Hz 8bit resolution
//ledcSetup(CHC, LEDC_BASE_FREQ, LEDC_TIMER_BIT); // ch0 490 Hz 8bit resolution
//ledcSetup(CHD, LEDC_BASE_FREQ, LEDC_TIMER_BIT); // ch1 490 Hz 8bit resolution
ledcSetup(CHE, LEDC_SERVO_BASE_FREQ, LEDC_SERVO_TIMER_BIT); // ch2 50 Hz 10bit resolution
// PWMピンのチャンネルをセット
ledcAttachPin(AIN1, CHA); //13 pin, ch0
ledcAttachPin(AIN2, CHB); //12 pin, ch1
//ledcAttachPin(BIN1, CHC); //0 pin, ch2
//ledcAttachPin(BIN2, CHD); //4 pin, ch3
ledcAttachPin(SERVO1, CHE); //32pin, ch4
//サーボの初期位置をセット
ledcWrite(CHE, posX_ex); // (中立)
//モーターの初期値をセット
ledcWrite(CHA, 0);
ledcWrite(CHB, 0);
//ledcWrite(CHC, 0);
//ledcWrite(CHD, 0);
} //end of setup
//Motor Drive の関数(DRV8833 Fast decay)
void forwardLeftMotor(int speedFL)//左正転
{
ledcWrite(CHA, 0);
ledcWrite(CHB, speedFL);
}
void backLeftMotor(int speedBL)//左逆転
{
ledcWrite(CHA, speedBL);
ledcWrite(CHB, 0);
}
/*void forwardRightMotor(int speedFR)//右正転
{
ledcWrite(CHC, speedFR);
ledcWrite(CHD, 0);
}
void backRightMotor(int speedBR)//右逆転
{
ledcWrite(CHC, 0);
ledcWrite(CHD, speedBR);
}*/
void stopLeftMotor() //停止(Brake)
{
ledcWrite(CHA, 255); //255
ledcWrite(CHB, 255); //255
}
/*void stopLeftMotor() //停止(空転)
{
ledcWrite(CHA, 0);
ledcWrite(CHB, 0);
}*/
void loop() {
if (Ps3.isConnected()){
if (Ps3.data.button.select){
Serial.println("select ボタン圧下");
v120 = 0;
v121 = 12;
if (v120 > 0) { vv120 = v120; } //if (v120 > 0)とif (v121 > 0)の意味は
if (v121 > 0) { vv121 = v121; } //ボタンを離した時(=0値)は ボタン状態変化とみなさないため
}
if (Ps3.data.button.start){
Serial.println("start ボタン圧下");
v120 = 0;
v121 = 3;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}
if (Ps3.data.button.up ){
Serial.println("up ボタン圧下");
v120 = 0;
v121 = 1;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}
if (Ps3.data.button.right){
Serial.println("right ボタン圧下");
v120 = 0;
v121 = 4;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}
if (Ps3.data.button.down ){
Serial.println("down ボタン圧下");
v120 = 0;
v121 = 2;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}
if (Ps3.data.button.left){
Serial.println("left ボタン圧下");
v120 = 0;
v121 = 8;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}
if (Ps3.data.button.l2 ){
Serial.println("l2 ボタン圧下");
v120 = 4;
v121 = 0;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}
if (Ps3.data.button.r2){
Serial.println("r2 ボタン圧下");
v120 = 16;
v121 = 0;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}
if (Ps3.data.button.l1 ){
Serial.println("l1 ボタン圧下");
v120 = 2;
v121 = 0;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}
if (Ps3.data.button.r1){
Serial.println("r1 ボタン圧下");
v120 = 8;
v121 = 0;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}
if (Ps3.data.button.cross){
Serial.println("cross ボタン圧下");
v120 = 0;
v121 = 32;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}
if (Ps3.data.button.square){
Serial.println("square ボタン圧下");
v120 = 0;
v121 = 1;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}
if (Ps3.data.button.triangle){
Serial.println("triangle ボタン圧下");
v120 = 0;
v121 = 16;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}
if (Ps3.data.button.circle){
Serial.println("circle ボタン圧下");
v120 = 0;
v121 = 64;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}
if (Ps3.data.button.ps){
Serial.println("ps ボタン圧下");
}
if (Ps3.data.button.l3 ){
Serial.println("l3 ボタン圧下");
}
if (Ps3.data.button.r3){
Serial.println("r3 ボタン圧下");
}
//チャタリングなのか、新しいボタン入力なのか はんだんするための処理
if ((vv120!=ex120)&&(vv121!=ex121)) {
//ボタンが押された時のvv120とvv121値が 前回押されたボタンの時と違う場合 vv120とvv121を次回のためにex120とex121に格納しておく
ex120 = vv120;
ex121 = vv121;
}
if ((vv120==ex120)&&(vv121!=ex121)) {
//ボタンが押された時のvv121値だけが 前回押されたボタンの時と違う場合 vv120をリセットしvv120、vv121を次回のためにex120とex121に格納しておく
vv120 = 0;
ex120 = vv120;
ex121 = vv121;
}
if ((vv120!=ex120)&&(vv121==ex121)) {
//ボタンが押された時のvv120値だけが 前回押されたボタンの時と違う場合 vv121をリセットしvv120、vv121を次回のためにex120とex121に格納しておく
vv121 = 0;
ex120 = vv120;
ex121 = vv121;
}
//PS3 Controller Buttonの値をbtnData変数に入れる
int btnData = vv120*256 + vv121;
Serial.print("btnData =,");
Serial.println(btnData);
//ジョイスティックの値で動かすプログラムをここに書く
int LX = (Ps3.data.analog.stick.lx)+128;
int LY = 255-((Ps3.data.analog.stick.ly)+128);
int RX = (Ps3.data.analog.stick.rx)+128; //servo1 右Joystick左右方向 (left-right turn)
int RY = 255-((Ps3.data.analog.stick.ry)+128); //right motor 右Joystick上下方向 (motor power & forward-backward)
Serial.print(" LX=,");
Serial.print(LX);
Serial.print(",LY=,");
Serial.print(LY);
Serial.print(",RX=,");
Serial.print(RX);
Serial.print(",RY=,");
Serial.println(RY);
//PS3 Controller 右joystick上下の値でモーターを動かす
//R2ボタン 0x1000押されたとき // モータースピード 低速 操舵角85%
if (btnData == 4096){
if (RY>160) //Forward前進
//if (RY>128) //Forward前進
{
speedFL = 6*(RY-128)/5; //0.6x3.7v Vm
forwardLeftMotor(speedFL);
}
if (RY<96) //Backward後進
{
speedBL =6*(128-RY)/5 ;
backLeftMotor(speedBL);
}
if (RY>=96 && RY<=160) //Stop motors
{
stopLeftMotor();
}
if (LX>138) { //right turn
posX= posX_ex-(LX-128)*(posX_ex - servo_angle_min_X)/150; //操舵角85%
ledcWrite(CHE, posX);
}
if (LX<118) { //left turn
posX= posX_ex+(128-LX)*(servo_angle_max_X - posX_ex)/150; //操舵角85%
ledcWrite(CHE, posX);
}
if (LX>=118 && LX<=138) { //neutral
posX= posX_ex;
ledcWrite(CHE, posX);
}
}
//R1ボタン 0x0800押されたとき // モータースピード 高速 3.7v,操舵角85%
if (btnData == 2048){
if (RY>160) //Forward前進
{
speedFL = 2*(RY-128); //3.7v
forwardLeftMotor(speedFL);
}
if (RY<96) //Backward後進
{
speedBL = 2*(128-RY);
backLeftMotor(speedBL);
}
if (RY>=96 && RY<=160) //Stop motors
{
stopLeftMotor();
}
if (LX>138) { //right turn
posX= posX_ex-(LX-128)*(posX_ex - servo_angle_min_X)/150; //操舵角85%
ledcWrite(CHE, posX);
}
if (LX<118) { //left turn
posX= posX_ex+(128-LX)*(servo_angle_max_X - posX_ex)/150; //操舵角85%
ledcWrite(CHE, posX);
}
if (LX>=118 && LX<=138) { //neutral
posX= posX_ex;
ledcWrite(CHE, posX);
}
}
//L2ボタン 0x0400押されたとき // 低速、操舵角100%
if (btnData == 1024){
if (RY>160) //Forward前進
{
speedFL = 6*(RY-128)/5;
forwardLeftMotor(speedFL);
}
if (RY<96) //Backward後進
{
speedBL = 6*(128-RY)/5;
backLeftMotor(speedBL);
}
if (RY>=96 && RY<=160) //Stop motors
{
stopLeftMotor();
}
if (LX>138) { //right turn
posX= posX_ex-(LX-128)*(posX_ex - servo_angle_min_X)/128; //操舵角100%
ledcWrite(CHE, posX);
}
if (LX<118) { //left turn
posX= posX_ex+(128-LX)*(servo_angle_max_X - posX_ex)/128; //操舵角100%
ledcWrite(CHE, posX);
}
if (LX>=118 && LX<=138) { //neutral
posX= posX_ex;
ledcWrite(CHE, posX);
}
}
//L1ボタン 0x0200押されたとき // 高速、操舵角100%
if (btnData == 512){
if (RY>160) //Forward前進
{
speedFL = 2*(RY-128); //3.7v
forwardLeftMotor(speedFL);
}
if (RY<96) //Backward後進
{
speedBL = 2*(128-RY);
backLeftMotor(speedBL);
}
if (RY>=96 && RY<=160) //Stop motors
{
stopLeftMotor();
}
if (LX>138) { //right turn
posX= posX_ex-(LX-128)*(posX_ex - servo_angle_min_X)/128;
ledcWrite(CHE, posX);
}
if (LX<118) { //left turn
posX= posX_ex+(128-LX)*(servo_angle_max_X - posX_ex)/128;
ledcWrite(CHE, posX);
}
if (LX>=118 && LX<=138) { //neutral
posX= posX_ex;
ledcWrite(CHE, posX);
}
}
}
delay(50);
} //loop関数の終わり、loop関数のはじめに戻ってloop関数をエンドレスで繰り返す
