added camera

include/data_source_ball.h

@@ -1,3 +1,4 @@
+#pragma once
 #include "data_source.h"
 
 class DataSourceBall : public DataSource{

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;

include/sensors.h

@@ -1,6 +1,7 @@
 #include <Arduino.h>
 #include "data_source_bno055.h"
 #include "data_source_ball.h"
+#include "data_source_camera.h"
 #include "motor.h"
 #include "drivecontroller.h"
 

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;
@@ -90,6 +90,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(){
     goalOrientation = digitalRead(SWITCH_SX);     //se HIGH attacco gialla, difendo blu
@@ -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);

src/main.cpp

@@ -17,6 +17,7 @@ void loop() {
   updateSensors();
   //should recenter using predefined values
  // drive->drive(0, 0, 0);
- compass->test();
+ //compass->test();
+  camera->test();
   delay(100);
 }

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();
 }