diff --git a/Main/Main.ino b/Main/Main.ino index b4dd646..f7c10e5 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -2,7 +2,8 @@ #include #include #include - +#include +#include //Declare library objects GY_85 IMU; @@ -39,6 +40,8 @@ long int m2Raw, m2RawLast; volatile bool M1_A_state, M1_B_state; volatile bool M2_A_state, M2_B_state; +//PS3 Controller variables +const char* _ps3Address = "18:5e:0f:92:00:6c"; void setup() { //Initialize serial @@ -78,8 +81,8 @@ void setup() { //Initialize differential drive inverse kinematics initMotors(); - // Initialize Remote control - initRemote(); + //Initialize PS3 controller connection + Ps3.begin(_ps3Address); } diff --git a/Main/interruptRemote.ino b/Main/interruptRemote.ino deleted file mode 100644 index 0dbfb08..0000000 --- a/Main/interruptRemote.ino +++ /dev/null @@ -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); -} diff --git a/Main/motorControl.ino b/Main/motorControl.ino index e753074..c958aa8 100644 --- a/Main/motorControl.ino +++ b/Main/motorControl.ino @@ -56,17 +56,9 @@ void motors() { //Calculate robot linear and angular velocity Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); - - // Remote control commands - if (pwm_time_ch1 == 0 && pwm_time_ch2 == 0){ - 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); - } - + //Get Control Commands + rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); + rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); // Speed Controller SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); diff --git a/Main/plot.ino b/Main/plot.ino index c0ea6b0..96b5ce0 100644 --- a/Main/plot.ino +++ b/Main/plot.ino @@ -72,4 +72,19 @@ void plot(){ // Serial.print(" "); // Serial.print("botAngVel:"); // 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); + // } }