reintroduces semi axis locking. Some bug with the lines make the robot stop when it gets on one. Tilt with vshaped mirror reintroduced

include/data_source_camera_conicmirror.h

@@ -7,10 +7,10 @@
 //#define unkn 0b01101001 
 #include "data_source.h"
 
-class DataSourceCamera : public DataSource{
+class DataSourceCameraConic : public DataSource{
 
     public:
-        DataSourceCamera(HardwareSerial* ser, int baud);
+        DataSourceCameraConic(HardwareSerial* ser, int baud);
         void test() override;
         void readSensor() override;
         int getValueAtk(bool);

include/data_source_camera_vshapedmirror.h

@@ -0,0 +1,26 @@
+#pragma once
+#include "data_source.h"
+
+class DataSourceCameraVShaped : public DataSource{
+
+    public:
+        DataSourceCameraVShaped(HardwareSerial* ser, int baud);
+        void postProcess() override;
+        void test() override;
+        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;
+        char value;
+        int startpY = 0;
+        int startpB = 0;
+        int endpY = 0; 
+        int endpB = 0;
+        int datavalid = 0;
+        String valStringY = ""; 
+        String valStringB = "";
+        bool negateB, negateY;
+};

include/drivecontroller.h

@@ -4,10 +4,11 @@
 #include "motor.h"
 
 //PID Constants
-
+#define KP 2.1 
-#define KP 0.9
 #define KI 0   
-#define KD 0
+#define KD 0.05
+
+#define UNLOCK_THRESH 800
 
 class DriveController{
 
@@ -23,7 +24,7 @@ class DriveController{
 
         int vxp, vyp, vxn, vyn;
         bool canUnlock;
-        elapsedMillis unlockTime;
+        unsigned long unlockTime;
 
     private:
         Motor* m1;

include/goalie.h

@@ -2,7 +2,7 @@
 
 #include "game.h"
 #include "sensors.h"
-#include "data_source_camera.h"
+#include "data_source_camera_vshapedmirror.h"
 
 #define GOALIE_ATKSPD_LAT  255
 #define GOALIE_ATKSPD_BAK  350

include/linesys_2019.h

@@ -36,5 +36,4 @@ class LineSys2019 : public LineSystem{
         elapsedMillis exitTimer;
         int outDir, outVel;
         byte linesens;
-        unsigned long unlockTime;
 };

include/sensors.h

@@ -10,7 +10,8 @@
 #include <Arduino.h>
 #include "data_source_bno055.h"
 #include "data_source_ball.h"
-#include "data_source_camera.h"
+#include "data_source_camera_conicmirror.h"
+#include "data_source_camera_vshapedmirror.h"
 #include "data_source_us.h"
 #include "motor.h"
 #include "ds_ctrl.h"
@@ -30,7 +31,7 @@ s_extr LineSys2019* linesCtrl;
 
 s_extr DataSourceBNO055* compass;
 s_extr DataSourceBall* ball;
-s_extr DataSourceCamera* camera;
+s_extr DataSourceCameraVShaped* camera;
 s_extr DriveController* drive;
 s_extr DataSourceBT* bt;
 

src/data_source_camera_conicmirror.cpp

@@ -1,9 +1,9 @@
-#include "data_source_camera.h"
+#include "data_source_camera_conicmirror.h"
 #include "sensors.h"
 
-DataSourceCamera::DataSourceCamera(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){}
+DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){}
 
-void DataSourceCamera :: readSensor(){
+void DataSourceCameraConic :: readSensor(){
   while(ser->available() > 0){
     value = (int)ser->read();
     //Serial.println(value);
@@ -31,8 +31,9 @@ void DataSourceCamera :: readSensor(){
         yAngle = (yAngle + 360) % 360;
         bAngle = (bAngle + 360) % 360;
 
-        yAngleFix = yAngle - compass->getValue()*0.9 ;
+        //Fixes with IMU
-        bAngleFix = bAngle - compass->getValue()*0.9  ;
+        yAngleFix = yAngle - compass->getValue()*0.85 ;
+        bAngleFix = bAngle - compass->getValue()*0.85  ;
 
         yDist = sqrt( (50-true_yy)*(50-true_yy) + (50-true_xy)*(50-true_xy) );
         bDist = sqrt( (50-true_yb)*(50-true_yb) + (50-true_xb)*(50-true_xb) );
@@ -52,14 +53,14 @@ void DataSourceCamera :: readSensor(){
 }
 
 
-int DataSourceCamera::getValueAtk(bool b){
+int DataSourceCameraConic::getValueAtk(bool b){
   return 0;
 }
-int DataSourceCamera::getValueDef(bool b){
+int DataSourceCameraConic::getValueDef(bool b){
   return 0;
 }
 
-void DataSourceCamera::test(){
+void DataSourceCameraConic::test(){
   goalOrientation = digitalRead(SWITCH_SX);     //se HIGH attacco gialla, difendo blu
   update();
     DEBUG.print(bAngle);

src/data_source_camera_vshapedmirror.cpp

@@ -0,0 +1,142 @@
+#include "data_source_camera_vshapedmirror.h"
+#include "sensors.h"
+
+DataSourceCameraVShaped::DataSourceCameraVShaped(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){}
+
+void DataSourceCameraVShaped :: readSensor(){
+  portx = 999;
+  while(ser->available() > 0) {
+    value = ser->read();
+    // if the incoming character is a 'Y', set the start packet flag
+    if (value == 'Y') {
+      startpY = 1;
+    }
+    // if the incoming character is a 'Y', set the start packet flag
+    if (value == 'B') {
+      startpB = 1;
+    }
+    // if the incoming character is a '.', set the end packet flag
+    if (value == 'y') {
+      endpY = 1;
+    }
+    // if the incoming character is a '.', set the end packet flag
+    if (value == 'b') {
+      endpB = 1;
+    }
+
+    if ((startpY == 1) && (endpY == 0)) {
+      if (isDigit(value)) {
+        // convert the incoming byte to a char and add it to the string:
+        valStringY += value;
+      }else if(value == '-'){
+        negateY = true;
+      }
+    }
+
+    if ((startpB == 1) && (endpB == 0)) {
+      if (isDigit(value)) {
+        // convert the incoming byte to a char and add it to the string:
+        valStringB += value;
+      }else if(value == '-'){
+        negateB = true;
+      }
+    }
+
+    if ((startpY == 1) && (endpY == 1)) {
+      valY = valStringY.toInt(); // valid data
+      if(negateY) valY *= -1;
+      valStringY = "";
+      startpY = 0;
+      endpY = 0;
+      negateY = false;
+      datavalid ++;
+    }
+    if ((startpB == 1) && (endpB == 1)) {
+      valB = valStringB.toInt(); // valid data
+      if(negateB) valB *= -1;
+      valStringB = "";
+      startpB = 0;
+      endpB = 0;
+      negateB = false;
+      datavalid ++;
+    }
+  }
+}
+
+void DataSourceCameraVShaped :: postProcess(){
+
+  if (valY != -74)
+    oldGoalY = valY;
+  if (valB != -74)
+    oldGoalB = valB;
+
+  if (valY == -74)
+    valY = oldGoalY;
+  if (valB == -74)
+    valB = oldGoalB;
+  
+  // entro qui solo se ho ricevuto i pacchetti completi sia del blu che del giallo
+  if (datavalid > 1 ) {
+    if(goalOrientation == 1){
+      //yellow goalpost
+      pAtk = valY;
+      pDef = valB * -1;
+    }else{
+      //blue goalpost
+      pAtk = valB;
+      pDef = valY * -1;
+    }
+
+    datavalid = 0;
+    cameraReady = 1;  //attivo flag di ricezione pacchetto
+  }
+}
+
+int DataSourceCameraVShaped::getValueAtk(bool fixed){
+  //attacco gialla
+  if(goalOrientation == HIGH){
+    if(fixed) return fixCamIMU(valY);
+    return valY;
+  }
+  //attacco blu
+  if(goalOrientation == LOW){
+    if(fixed) return fixCamIMU(valB);
+    return valB;
+  }
+}
+
+int DataSourceCameraVShaped::getValueDef(bool fixed){
+  //difendo gialla
+  if(goalOrientation == HIGH){
+    if(fixed) return fixCamIMU(valY);
+    return valY;
+  }
+  //difendo blu
+  if(goalOrientation == LOW){
+    if(fixed) return fixCamIMU(valB);
+    return valB;
+  }
+}
+
+void DataSourceCameraVShaped::test(){
+    goalOrientation = digitalRead(SWITCH_SX);     //se HIGH attacco gialla, difendo blu
+    
+
+    DEBUG.print(pAtk);
+    DEBUG.print(" | ");
+    DEBUG.print(fixCamIMU(pAtk));
+    DEBUG.print(" --- ");
+
+    DEBUG.print(pDef);
+    DEBUG.print(" | ");
+    DEBUG.println(fixCamIMU(pDef));
+    delay(100);
+}
+
+int DataSourceCameraVShaped::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);
+
+    return d + imuOff;
+}

src/drivecontroller.cpp

@@ -32,6 +32,13 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
     errorePre = 0;
     pidfactor = 0;
     integral = 0;
+    canUnlock = true;
+    unlockTime = 0;
+
+    vxp = 0;
+    vxn = 0;
+    vyp = 0;
+    vyn = 0;
 }
 
 void DriveController::prepareDrive(int dir, int speed, int tilt){
@@ -52,6 +59,16 @@ void DriveController::drive(int dir, int speed, int tilt){
     vx = ((speed * cosins[dir]));
     vy = ((-speed * sins[dir]));
 
+    if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) {
+        vxn = 0;
+        vxp = 0;
+        vyp = 0;
+        vyn = 0;
+    }
+
+    if((vy > 0 && vxn == 1) || (vy < 0 && vxp == 1)) vy = 0;
+    if((vx > 0 && vyp == 1) || (vx < 0 && vyn == 1)) vx = 0; 
+
     speed1 = ((vx * sins[m1->angle] ) + (vy * cosins[m1->angle] ));
     speed2 = ((vx * sins[m2->angle]) + (vy * cosins[m2->angle]));
     speed3 = -(speed1);

src/goalie.cpp

@@ -34,51 +34,11 @@ void Goalie::goalie(int plusang) {
     if(ball->dir < 0) ball->dir = ball->dir + 360;
     else ball->dir = ball->dir;
     ball->b = ball->dir;
-    drive->prepareDrive(ball->dir, 300, 0);
-  }
- /*  drive->speed = 300;
-  drive->dir = drive->dir; */
-  /* if(ball->angle >= 350 || ball->angle <= 10)  {
-    if(ball->distance > 190) atk_direction = 0;
-    else atk_direction = ball->angle;
-    atk_speed = GOALIE_ATKSPD_FRT;
-  }
 
-  if(ball->angle >= 90 && ball->angle <= 270) {
+    storcimentoPorta();
-    this->ballBack();
+    if(ball->angle > 340 || ball->angle < 20) drive->prepareDrive(ball->dir, 350, cstorc);
-    atk_speed = GOALIE_ATKSPD_BAK;                   
+    else  drive->prepareDrive(ball->dir, 350, 0);
   }
-  if(ball->angle > 10   && ball->angle <  30)   {
-    atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1;
-    atk_speed = GOALIE_ATKSPD_LAT;
-  }
-  if(ball->angle >= 30 && ball->angle <  45)    {
-    atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2;
-    atk_speed = GOALIE_ATKSPD_LAT;
-  }
-  if(ball->angle >= 45 && ball->angle <  90)    {
-    atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3;
-    atk_speed = GOALIE_ATKSPD_LAT;
-  }
-  if(ball->angle > 270 && ball->angle <= 315)   {
-    atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3_COR;
-    atk_speed = GOALIE_ATKSPD_LAT;
-  }
-  if(ball->angle > 315 && ball->angle <= 330)   {
-    atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2_COR;
-    atk_speed = GOALIE_ATKSPD_LAT;
-  }
-  if(ball->angle > 330 && ball->angle <  350)   {
-    atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1_COR;
-    atk_speed = GOALIE_ATKSPD_LAT;
-  }
-
-  this->storcimentoPorta();
-  if((ball->angle >= 330 || ball->angle <= 30) && ball->distance > 190) {      //storcimento
-    atk_speed = GOALIE_ATKSPD_STRK;                                             //dove i gigahertz hanno fallito
-    drive->prepareDrive(atk_direction, atk_speed, cstorc);
-  }
-  else  drive->prepareDrive(atk_direction, atk_speed); */
 }
 
 void Goalie::storcimentoPorta() {
@@ -86,18 +46,4 @@ void Goalie::storcimentoPorta() {
   else if (camera->getValueAtk(true)  < -3) cstorc-=9;
   else cstorc *= 0.7;
   cstorc = constrain(cstorc, -45, 45);
-}
-
-void Goalie::ballBack() {
-
-/*   if(ball->distance > 130) ball->plusang = GOALIE_ATKDIR_PLUSANGBAK;
-  else ball->plusang = 0;
-  
-  if(ball->angle > 180) ball->degrees2 = ball->angle - 360;
-  else ball->degrees2 = ball->angle;
-  if(ball->degrees2 > 0) ball->dir = ball->angle + ball->plusang;              //45 con 8 ruote
-  else ball->dir = ball->angle - ball->plusang;                                 //45 con 8 ruote
-  if(ball->dir < 0) ball->dir = ball->dir + 360;
-  else ball->dir = ball->dir;
-  atk_direction = ball->dir; */
 }

src/linesys_2019.cpp

@@ -78,7 +78,7 @@ void LineSys2019::outOfBounds(){
   if (exitTimer <= EXTIME){
     //fase di rientro
     if(linesens == 15) linesens = linesensOldY | linesensOldX;        //ZOZZATA MAXIMA
-    unlockTime = millis();
+    drive->unlockTime = millis();
 
     if(linesens == 1) outDir = 180;
     else if(linesens == 2) outDir = 270;
@@ -153,7 +153,7 @@ void LineSys2019::outOfBounds(){
       if(linesensOldY == 4) drive->vyn = 1;
       else if(linesensOldY == 1) drive->vyp = 1;
     }
-
+    drive->canUnlock = true;
     linesens = 0;
     linesensOldY = 0;
     linesensOldX = 0;

src/main.cpp

@@ -16,12 +16,10 @@ void setup() {
 
 void loop() {
   updateSensors();
-  //camera->test();
+  // camera->test();
   goalie->play(role==1);
   keeper->play(role==0);  
 
-camera->test();
-  
   // Last thing to do: movement  
   drive->drivePrepared();  
 } 

src/sensors.cpp

@@ -15,7 +15,7 @@ void initSensors(){
     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);
+    camera = new DataSourceCameraVShaped(&Serial2, 19200);
     usCtrl = new DataSourceCtrl(dUs);
     bt = new DataSourceBT(&Serial3, 115200);
  }