Fixed camera angle reading from both openmv and teensy. Tilt from goalie kinda working now

include/data_source_bno055.h

@@ -4,6 +4,8 @@
 #include <Adafruit_BNO055.h>
 #include <Arduino.h>
 
+#define DATA_CLOCK 10
+
 class DataSourceBNO055 : public DataSource{
 
     public:
@@ -12,5 +14,6 @@ class DataSourceBNO055 : public DataSource{
         
     public:
         Adafruit_BNO055 bno;
+        unsigned long lastTime;
 
 };

include/data_source_camera_conicmirror.h

@@ -19,7 +19,7 @@ class DataSourceCameraConic : public DataSource{
         int yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist;
 
         int count = 0, unkn_counter;
-        byte xb, yb, xy, yy, true_xb, true_xy, true_yb, true_yy, calc_xb, calc_yb, calc_xy, calc_yy;
+        int xb, yb, xy, yy, true_xb, true_xy, true_yb, true_yy, calc_xb, calc_yb, calc_xy, calc_yy;
         bool data_received = false, start = false, end = false;
 
         int goalOrientation, pAtk, pDef;

include/drivecontroller.h

@@ -17,7 +17,8 @@ class DriveController{
         DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_);
         
         void drive(int dir=0, int speed=0, int tilt=0);
-        void prepareDrive(int dir=0, int speed=0, int tilt=0);
+        void prepareDrive(int dir, int speed, int tilt);
+        void prepareDrive(int dir, int speed);
         void drivePrepared();
         float updatePid();
         float torad(float f);

include/sensors.h

@@ -31,7 +31,7 @@ s_extr LineSys2019* linesCtrl;
 
 s_extr DataSourceBNO055* compass;
 s_extr DataSourceBall* ball;
-s_extr DataSourceCameraVShaped* camera;
+s_extr DataSourceCameraConic* camera;
 s_extr DriveController* drive;
 s_extr DataSourceBT* bt;
 

include/vars.h

@@ -1,5 +1,5 @@
 #pragma once
-#define DEBUG Serial
+#define DEBUG Serial3
 
 #define LED_R 20
 #define LED_Y 17

src/data_source_bno055.cpp

@@ -10,9 +10,13 @@ DataSourceBNO055::DataSourceBNO055(){
   bno.setExtCrystalUse(true);
   //loaded = true;
   value = 0;
+  lastTime = 0;
 }
 
 void DataSourceBNO055::readSensor(){
+  if(millis() - lastTime > DATA_CLOCK){
     imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
     this->value = (int) euler.x();
+    lastTime = millis();
+  }
 }

src/data_source_bt.cpp

@@ -1,7 +1,7 @@
 #include "data_source_bt.h"
 
 DataSourceBT :: DataSourceBT(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){
-    connect();
+    // connect();
 }
 
 void DataSourceBT :: connect(){

src/data_source_camera_conicmirror.cpp

@@ -1,7 +1,25 @@
 #include "data_source_camera_conicmirror.h"
 #include "sensors.h"
 
-DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){}
+DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){
+  true_xb = 0;
+  true_yb = 0;
+  true_xy = 0;
+  true_yy = 0;
+  xb = 0;
+  yb = 0;
+  xy = 0;
+  yy = 0;
+  start = false;
+  data_received = false;
+  end = false;
+  yAngle = 0;
+  yAngleFix = 0;
+  yDist = 0;
+  bAngle = 0;
+  bAngleFix = 0;
+  bDist = 0;
+}
 
 void DataSourceCameraConic :: readSensor(){
   while(ser->available() > 0){
@@ -16,27 +34,24 @@ void DataSourceCameraConic :: readSensor(){
       if(count=4 && start==true) {
         data_received=true;
 
-        true_xb = xb;
+        true_xb = xb-50;
-        true_yb = yb;
+        true_yb = 50-yb;
-        true_xy = xy;
+        true_xy = xy-50;
-        true_yy = yy;
+        true_yy = 50-yy;
 
         //Remap from [0,100] to [-50, +49] to correctly compute angles and distances and calculate them
-        yAngle = atan2(50-true_yy, 50-true_xy) * 180 / 3.14;
+        yAngle = -90-(atan2(true_yy, true_xy) * 180 /3.14);
-        bAngle = atan2(50-true_yb, 50-true_xb) * 180 / 3.14;
+        bAngle = -90-(atan2(true_yb, true_xb)* 180 /3.14);
-        //Subtract 90 to bring the angles back to euler angles (0 in front)
-        yAngle = 90 - yAngle;
-        bAngle = 90 - bAngle;
         //Now cast angles to [0, 359] domain angle flip them
-        yAngle = (yAngle + 360) % 360;
+        yAngle = (yAngle+360)%360;
-        bAngle = (bAngle + 360) % 360;
+        bAngle = (bAngle+360)%360;
 
         //Fixes with IMU
-        yAngleFix = yAngle - compass->getValue()*0.85 ;
+        yAngleFix = ((int)(yAngle - compass->getValue()*0.35) + 360) % 360  ;
-        bAngleFix = bAngle - compass->getValue()*0.85  ;
+        bAngleFix = ((int)(bAngle - compass->getValue()*0.35) + 360) % 360 ;
 
-        yDist = sqrt( (50-true_yy)*(50-true_yy) + (50-true_xy)*(50-true_xy) );
+        yDist = sqrt( (true_yy-50)*(true_yy-50) + (50-true_xy)*(50-true_xy) );
-        bDist = sqrt( (50-true_yb)*(50-true_yb) + (50-true_xb)*(50-true_xb) );
+        bDist = sqrt( (true_yb-50)*(true_yb-50) + (50-true_xb)*(50-true_xb) );
       }
       end=true;
       start=false;
@@ -53,16 +68,19 @@ void DataSourceCameraConic :: readSensor(){
 }
 
 
-int DataSourceCameraConic::getValueAtk(bool b){
+int DataSourceCameraConic::getValueAtk(bool fixed){
-  return 0;
+  if(fixed) return goalOrientation == HIGH ? yAngleFix : bAngleFix;
+  else return goalOrientation == HIGH ? yAngle : bAngle;
 }
-int DataSourceCameraConic::getValueDef(bool b){
+int DataSourceCameraConic::getValueDef(bool fixed){
-  return 0;
+  if(fixed) return goalOrientation == LOW ? yAngleFix : bAngleFix;
+  else return goalOrientation == LOW ? yAngle : bAngle;
 }
 
 void DataSourceCameraConic::test(){
   goalOrientation = digitalRead(SWITCH_SX);     //se HIGH attacco gialla, difendo blu
   update();
+    DEBUG.print("Blue: ");
     DEBUG.print(bAngle);
     DEBUG.print(" | ");
     DEBUG.print(bAngleFix);
@@ -70,12 +88,14 @@ void DataSourceCameraConic::test(){
     DEBUG.println(bDist);
     DEBUG.println(" --- ");
 
+    DEBUG.print("Yellow: ");
     DEBUG.print(yAngle);
     DEBUG.print(" | ");
     DEBUG.print(yAngleFix); 
     DEBUG.print(" | ");
     DEBUG.println(yDist);
     DEBUG.println("---------------");
+    DEBUG.print("Data: ");
     DEBUG.print(true_xb);
     DEBUG.print("|");
     DEBUG.print(true_yb);

src/drivecontroller.cpp

@@ -47,6 +47,11 @@ void DriveController::prepareDrive(int dir, int speed, int tilt){
     pTilt = tilt;
 }
 
+void DriveController::prepareDrive(int dir, int speed){
+    pDir = dir;
+    pSpeed = speed;
+}
+
 void DriveController::drivePrepared(){
     drive(pDir, pSpeed, pTilt);
 }

src/goalie.cpp

@@ -36,14 +36,17 @@ void Goalie::goalie(int plusang) {
     ball->b = ball->dir;
 
     storcimentoPorta();
-    if(ball->angle > 340 || ball->angle < 20) drive->prepareDrive(ball->dir, 350, cstorc);
+    if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(ball->dir, 150, cstorc);
-    else  drive->prepareDrive(ball->dir, 350, 0);
+    else {
+      drive->prepareDrive(ball->dir, 150, 0);
+      cstorc = 0;
+    }
   }
 }
 
 void Goalie::storcimentoPorta() {
-  if (camera->getValueAtk(true) >= 3) cstorc+=9;
+  if (camera->getValueAtk(false) >= 10 && camera->getValueAtk(false) <= 90) cstorc+=9;
-  else if (camera->getValueAtk(true)  < -3) cstorc-=9;
+  else if (camera->getValueAtk(false)  <= 350 && camera->getValueAtk(false) >= 270) cstorc-=9;
-  else cstorc *= 0.7;
+  // else cstorc *= 0.7;
   cstorc = constrain(cstorc, -45, 45);
 }

src/linesys_2019.cpp

@@ -101,7 +101,7 @@ void LineSys2019::outOfBounds(){
       else if(linesensOldY == 1) outDir = 180;
     }
 
-    drive->prepareDrive(outDir, LINES_EXIT_SPD, 0);
+    drive->prepareDrive(outDir, LINES_EXIT_SPD);
     tookLine = true;
   }else{
     //fine rientro

src/main.cpp

@@ -4,19 +4,20 @@
 #include "games.h"
 
 void setup() {
-  delay(1000);
+  delay(1500);
 
   DEBUG.begin(9600);
   initSensors();
   initGames();
   
-  delay(2000);
+  delay(1500);
 }
 
 
 void loop() {
   updateSensors();
   // camera->test();
+  // compass->test();
   goalie->play(role==1);
   keeper->play(role==0);  
 

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

utility/OpenMV/conic_eff.py

@@ -39,8 +39,8 @@ blue_led.on()
 
 
 
-thresholds = [  (44, 100, -4, 48, 11, 74),    # thresholds yellow goal
+thresholds = [  (49, 99, -24, 17, 23, 64),    # thresholds yellow goal
-                (16, 60, -4, 48, -73, -22)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (-128,-128,-128,-128,-128,-128)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)
 
@@ -61,10 +61,10 @@ sensor.reset()
 sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QQVGA)
 sensor.set_contrast(+3)
-sensor.set_saturation(+1)
+sensor.set_saturation(0)
-sensor.set_brightness(-1)
+sensor.set_brightness(-3)
 sensor.set_quality(0)
-sensor.set_auto_exposure(False, 12000)
+sensor.set_auto_exposure(False, 6000)
 sensor.set_auto_gain(True)
 sensor.skip_frames(time = 300)
 
@@ -84,7 +84,7 @@ while(True):
     tt_blue = [(0,999,0,2)]       ## creo una lista di tuple per il blue, valore x = 999 : non trovata
 
     img = sensor.snapshot()
-    for blob in img.find_blobs(thresholds, pixels_threshold=75, area_threshold=115, merge = True):
+    for blob in img.find_blobs(thresholds, pixels_threshold=75, area_threshold=130, merge = True):
         img.draw_rectangle(blob.rect())
         img.draw_cross(blob.cx(), blob.cy())
 
@@ -141,9 +141,9 @@ while(True):
 
 
     uart.write(START_BYTE)
-    uart.write(s_ycx)
-    uart.write(s_ycy)
     uart.write(s_bcx)
     uart.write(s_bcy)
+    uart.write(s_ycx)
+    uart.write(s_ycy)
     uart.write(END_BYTE)