diff --git a/include/data_source_bt.h b/include/data_source_bt.h new file mode 100644 index 0000000..35fdf35 --- /dev/null +++ b/include/data_source_bt.h @@ -0,0 +1,14 @@ +#pragma once +#include "data_source.h" +#include "vars.h" + +class DataSourceBT : public DataSource{ + + public: + DataSourceBT(HardwareSerial* ser, int baud); + void test() override; + void connect(); + void reconnect(); + + bool b, comrade; +}; \ No newline at end of file diff --git a/include/sensors.h b/include/sensors.h index d9d5e91..b50067b 100755 --- a/include/sensors.h +++ b/include/sensors.h @@ -16,6 +16,7 @@ #include "motor.h" #include "ds_ctrl.h" #include "drivecontroller.h" +#include "data_source_bt.h" void initSensors(); void updateSensors(); @@ -31,5 +32,6 @@ s_extr DataSourceBNO055* compass; s_extr DataSourceBall* ball; s_extr DataSourceCamera* camera; s_extr DriveController* drive; +s_extr DataSourceBT* bt; s_extr int role; \ No newline at end of file diff --git a/src/data_source_bt.cpp b/src/data_source_bt.cpp new file mode 100644 index 0000000..565f260 --- /dev/null +++ b/src/data_source_bt.cpp @@ -0,0 +1,38 @@ +#include "data_source_bt.h" + +DataSourceBT :: DataSourceBT(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){ + connect(); +} + +void DataSourceBT :: connect(){ + Serial3.print("$"); + Serial3.print("$"); + Serial3.print("$"); + delay(100); + Serial3.println("C"); +} + +void DataSourceBT :: reconnect(){ + if(!comrade){ + if(!b){ + Serial3.print("$"); + Serial3.print("$"); + Serial3.print("$"); + }else{ + Serial3.println("C"); + } + + b = !b; + }else{ + Serial3.println("---"); + } +} + +void DataSourceBT :: test(){ + if (DEBUG.available()) { + DEBUG.println((char)DEBUG.read()); + } + if (Serial3.available()) { + Serial3.write((char)Serial3.read()); + } +} \ No newline at end of file diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index db4b651..af2653d 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -34,7 +34,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_) integral = 0; } -void DriveController::prepareDrive(int dir=0, int speed=0, int tilt=0){ +void DriveController::prepareDrive(int dir, int speed, int tilt){ pDir = dir; pSpeed = speed; pTilt = tilt; @@ -48,7 +48,7 @@ float DriveController::torad(float f){ return (f * PI / 180.0); } -void DriveController::drive(int dir=0, int speed=0, int tilt=0){ +void DriveController::drive(int dir, int speed, int tilt){ vx = ((speed * cosins[dir])); vy = ((-speed * sins[dir])); diff --git a/src/game.cpp b/src/game.cpp index ee521a6..dfbd250 100644 --- a/src/game.cpp +++ b/src/game.cpp @@ -2,6 +2,6 @@ Game::Game() {} -void Game::play(bool condition=true){ +void Game::play(bool condition){ if(condition) realPlay(); } \ No newline at end of file diff --git a/src/goalie.cpp b/src/goalie.cpp index 1917c36..485cc60 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -12,6 +12,7 @@ void Goalie::realPlay(){ } void Goalie::goalie() { + compass->readSensor(); if(ball->angle >= 350 || ball->angle <= 10) { if(ball->distance > 190) atk_direction = 0; else atk_direction = ball->angle; diff --git a/src/main.cpp b/src/main.cpp index 502ecd3..4bf6849 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -24,5 +24,7 @@ void loop() { linesCtrl->update(); // Last thing to do: movement - drive->drivePrepared(); + drive->drivePrepared(); + +/* compass->test(); */ } diff --git a/src/sensors.cpp b/src/sensors.cpp index 20271b9..6fb504f 100755 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -17,6 +17,7 @@ void initSensors(){ camera = new DataSourceCamera(&Serial2, 19200); usCtrl = new DataSourceCtrl(dUs); linesCtrl = new DataSourceCtrlLines(lIn, lOut); + bt = new DataSourceBT(&Serial3, 115200); /*game = new Game(); goalie = new Goalie();