Stedd
/
BalanceBot
1
0
Fork 0
Code Issues 6 Pull Requests Packages Projects 1 Releases Wiki Activity

Added PS3 controller

This commit is contained in:
Stedd 2023-10-18 21:48:18 +02:00
parent 248cd4dc68
commit 23c84d2e41
4 changed files with 24 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Main/Main.ino
View File

@ -2,7 +2,8 @@
#include <GY_85.h> #include <GY_85.h>
#include <Wire.h> #include <Wire.h>
#include <MatrixMath.h> #include <MatrixMath.h>
#include <Math.h>
#include <Ps3Controller.h>
//Declare library objects //Declare library objects
GY_85 IMU; GY_85 IMU;
@ -39,6 +40,8 @@ long int m2Raw, m2RawLast;
volatile bool M1_A_state, M1_B_state; volatile bool M1_A_state, M1_B_state;
volatile bool M2_A_state, M2_B_state; volatile bool M2_A_state, M2_B_state;
//PS3 Controller variables
const char* _ps3Address = "18:5e:0f:92:00:6c";
void setup() { void setup() {
//Initialize serial //Initialize serial
@ -78,8 +81,8 @@ void setup() {
//Initialize differential drive inverse kinematics //Initialize differential drive inverse kinematics
initMotors(); initMotors();
// Initialize Remote control //Initialize PS3 controller connection
initRemote(); Ps3.begin(_ps3Address);
} }

36
Main/interruptRemote.ino
View File

@ -1,36 +0,0 @@
//Variables
const byte NO_CHANNELS = 2;
const byte CHANNEL_PINS[] = {12, 14};
volatile unsigned int interruptTime_ch1, interruptTimeLast_ch1;
volatile unsigned int interruptTime_ch2, interruptTimeLast_ch2;
volatile unsigned int pwm_time_ch1, pwm_time_ch2;
void ch1_interrupt() {
interruptTime_ch1 = micros();
if (interruptTime_ch1 >interruptTimeLast_ch1 && (interruptTime_ch1 - interruptTimeLast_ch1)< 2100){
pwm_time_ch1 = interruptTime_ch1 - interruptTimeLast_ch1;
}
interruptTimeLast_ch1 = interruptTime_ch1;
}
void ch2_interrupt() {
interruptTime_ch2 = micros();
if (interruptTime_ch2 > interruptTimeLast_ch2 && (interruptTime_ch2 - interruptTimeLast_ch2)< 2100 ){
pwm_time_ch2 = interruptTime_ch2 - interruptTimeLast_ch2;
}
interruptTimeLast_ch2 = interruptTime_ch2;
}
void initRemote(){
//Ch1
pinMode(CHANNEL_PINS[0], INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(CHANNEL_PINS[0]), ch1_interrupt, CHANGE);
//Ch2
pinMode(CHANNEL_PINS[1], INPUT);
attachInterrupt(digitalPinToInterrupt(CHANNEL_PINS[1]), ch2_interrupt, CHANGE);
}

14
Main/motorControl.ino
View File

@ -56,17 +56,9 @@ void motors() {
//Calculate robot linear and angular velocity //Calculate robot linear and angular velocity
Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix);
//Get Control Commands
// Remote control commands rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75);
if (pwm_time_ch1 == 0 && pwm_time_ch2 == 0){ rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35);
rem_turn_speed_ref = 0;
rem_speed_ref = 0;
}
else{
rem_turn_speed_ref = floatMap(pwm_time_ch1, 992.0, 2015.0, -3.75, 3.75);
rem_speed_ref = floatMap(pwm_time_ch2, 982.0, 2005.0, -0.35, 0.35);
}
// Speed Controller // Speed Controller
SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC);

15
Main/plot.ino
View File

@ -72,4 +72,19 @@ void plot(){
// Serial.print(" "); // Serial.print(" ");
// Serial.print("botAngVel:"); // Serial.print("botAngVel:");
// Serial.println(vel_Matrix[1][0]); // Serial.println(vel_Matrix[1][0]);
// //PS3 Controller
// if (Ps3.isConnected()) {
// Serial.print("lx:");
// Serial.print(Ps3.data.analog.stick.lx);
// Serial.print(",");
// Serial.print("ly:");
// Serial.print(Ps3.data.analog.stick.ly);
// Serial.print(",");
// Serial.print("rx:");
// Serial.print(Ps3.data.analog.stick.rx);
// Serial.print(",");
// Serial.print("ry:");
// Serial.println(Ps3.data.analog.stick.ry);
// }
} }