From 3d012ac59f87847998afa46892cb97a7f4837535 Mon Sep 17 00:00:00 2001 From: u-siri-ous Date: Mon, 18 Nov 2019 18:11:26 +0100 Subject: [PATCH] added camera --- include/data_source_ball.h | 1 + include/data_source_camera.h | 8 +++-- include/sensors.h | 1 + src/data_source_camera.cpp | 61 ++++++++++++++++++++++++++---------- src/main.cpp | 5 +-- src/sensors.cpp | 10 ++++-- 6 files changed, 63 insertions(+), 23 deletions(-) diff --git a/include/data_source_ball.h b/include/data_source_ball.h index c6b7886..4199888 100644 --- a/include/data_source_ball.h +++ b/include/data_source_ball.h @@ -1,3 +1,4 @@ +#pragma once #include "data_source.h" class DataSourceBall : public DataSource{ diff --git a/include/data_source_camera.h b/include/data_source_camera.h index 6ed0f02..d4725f9 100644 --- a/include/data_source_camera.h +++ b/include/data_source_camera.h @@ -1,3 +1,4 @@ +#pragma once #include "data_source.h" class DataSourceCamera : public DataSource{ @@ -6,10 +7,13 @@ class DataSourceCamera : public DataSource{ DataSourceCamera(HardwareSerial* ser, int baud); void postProcess() override; void test() override; - void fixCamIMU(int); + int fixCamIMU(int); + void readSensor() override; + int getValueAtk(bool); + int getValueDef(bool); int goalOrientation, pAtk, pDef, imuOff, portx, valX, valY, valB, oldGoalX, oldGoalY, oldGoalB; - int cameraready; + int cameraReady; char value; int startpY = 0; int startpB = 0; diff --git a/include/sensors.h b/include/sensors.h index 1334c70..9a3a690 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -1,6 +1,7 @@ #include #include "data_source_bno055.h" #include "data_source_ball.h" +#include "data_source_camera.h" #include "motor.h" #include "drivecontroller.h" diff --git a/src/data_source_camera.cpp b/src/data_source_camera.cpp index b6c9701..d4f2bd6 100644 --- a/src/data_source_camera.cpp +++ b/src/data_source_camera.cpp @@ -5,41 +5,39 @@ DataSourceCamera::DataSourceCamera(HardwareSerial* ser_, int baud) : DataSource( void DataSourceCamera :: readSensor(){ portx = 999; - while(ser->available() > 0) value = ser->read(); -} - -void DataSourceCamera :: postProcess(){ + while(ser->available() > 0) { + value = ser->read(); // if the incoming character is a 'Y', set the start packet flag - if (inChar == 'Y') { + if (value == 'Y') { startpY = 1; } // if the incoming character is a 'Y', set the start packet flag - if (inChar == 'B') { + if (value == 'B') { startpB = 1; } // if the incoming character is a '.', set the end packet flag - if (inChar == 'y') { + if (value == 'y') { endpY = 1; } // if the incoming character is a '.', set the end packet flag - if (inChar == 'b') { + if (value == 'b') { endpB = 1; } if ((startpY == 1) && (endpY == 0)) { - if (isDigit(inChar)) { + if (isDigit(value)) { // convert the incoming byte to a char and add it to the string: - valStringY += inChar; - }else if(inChar == '-'){ + valStringY += value; + }else if(value == '-'){ negateY = true; } } if ((startpB == 1) && (endpB == 0)) { - if (isDigit(inChar)) { + if (isDigit(value)) { // convert the incoming byte to a char and add it to the string: - valStringB += inChar; - }else if(inChar == '-'){ + valStringB += value; + }else if(value == '-'){ negateB = true; } } @@ -62,8 +60,10 @@ void DataSourceCamera :: postProcess(){ negateB = false; datavalid ++; } +} +} - } // end of while +void DataSourceCamera :: postProcess(){ if (valY != -74) oldGoalY = valY; @@ -77,7 +77,7 @@ void DataSourceCamera :: postProcess(){ // entro qui solo se ho ricevuto i pacchetti completi sia del blu che del giallo if (datavalid > 1 ) { - if(goal_orientation == 1){ + if(goalOrientation == 1){ //yellow goalpost pAtk = valY; pDef = valB * -1; @@ -89,6 +89,33 @@ void DataSourceCamera :: postProcess(){ datavalid = 0; cameraReady = 1; //attivo flag di ricezione pacchetto + } +} + +int DataSourceCamera :: getValueAtk(bool fixed){ + //attacco gialla + if(digitalRead(SWITCH_DX) == HIGH){ + if(fixed) return fixCamIMU(valY); + return valY; + } + //attacco blu + if(digitalRead(SWITCH_DX) == LOW){ + if(fixed) return fixCamIMU(valB); + return valB; + } +} + +int DataSourceCamera :: getValueDef(bool fixed){ + //difendo gialla + if(digitalRead(SWITCH_DX) == HIGH){ + if(fixed) return fixCamIMU(valY); + return valY; + } + //difendo blu + if(digitalRead(SWITCH_DX) == LOW){ + if(fixed) return fixCamIMU(valB); + return valB; + } } void DataSourceCamera :: test(){ @@ -106,7 +133,7 @@ void DataSourceCamera :: test(){ delay(100); } -void DataSourceCamera :: fixCamIMU(int d){ +int DataSourceCamera :: fixCamIMU(int d){ if(compass->getValue() > 0 && compass->getValue() < 180) imuOff = compass->getValue(); else if (compass->getValue() <= 360 && compass->getValue() >= 180) imuOff = compass->getValue() - 360; imuOff = constrain(imuOff*0.8, -30, 30); diff --git a/src/main.cpp b/src/main.cpp index bec2e0b..96dbd77 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -17,6 +17,7 @@ void loop() { updateSensors(); //should recenter using predefined values // drive->drive(0, 0, 0); - compass->test(); - delay(100); + //compass->test(); + camera->test(); + delay(100); } \ No newline at end of file diff --git a/src/sensors.cpp b/src/sensors.cpp index 98842ee..7305d70 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -2,12 +2,18 @@ #include "sensors.h" void initSensors(){ - compass = new DataSourceBNO055(); drive = new DriveController(new Motor(12, 11, 2, 45),new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315)); + compass = new DataSourceBNO055(); ball = new DataSourceBall(&Serial4, 57600); camera = new DataSourceCamera(&Serial2, 19200); } void updateSensors(){ - compass->readSensor(); + + pinMode(SWITCH_DX, INPUT); + pinMode(SWITCH_SX, INPUT); + + compass->update(); + ball->update(); + camera->update(); } \ No newline at end of file