Angles working?

include/data_source_camera.h

@@ -1,6 +1,9 @@
 #pragma once
 #define startp 105
 #define endp 115
+//Coords are mapped from 0 up to this value
+#define MAP_MAX 100
+#define HALF_MAP_MAX  50
 //#define unkn 0b01101001 
 #include "data_source.h"
 
@@ -8,9 +11,7 @@ class DataSourceCamera : public DataSource{
 
     public:
         DataSourceCamera(HardwareSerial* ser, int baud);
-        void postProcess() override;
         void test() override;
-        int fixCamIMU(int);
         void readSensor() override;
         int getValueAtk(bool);
         int getValueDef(bool);
@@ -18,18 +19,9 @@ class DataSourceCamera : 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;
+        byte 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, imuOff, portx, valX, valY, valB, oldGoalX, oldGoalY, oldGoalB;
+        int goalOrientation, pAtk, pDef;
-        int cameraReady;
         int value;
-        int startpY = 0;
-        int startpB = 0;
-        int endpY = 0; 
-        int endpB = 0;
-        int datavalid = 0;
-        String valStringY = ""; 
-        String valStringB = "";
-        bool negateB, negateY; 
 };

src/data_source_camera.cpp

@@ -20,20 +20,21 @@ void DataSourceCamera :: readSensor(){
         true_xy = xy;
         true_yy = yy;
 
-        //Remap to [-50, +49] to correctly compute angles and distances
+        //Remap from [0,100] to [-50, +49] to correctly compute angles and distances and calculate them
-        true_xb -= 50;
+        yAngle = atan2(50-true_yy, 50-true_xy) * 180 / 3.14;
-        true_yb -= 50;
+        bAngle = atan2(50-true_yb, 50-true_xb) * 180 / 3.14;
-        true_xy -= 50;
+        //Subtract 90 to bring the angles back to euler angles (0 in front)
-        true_yy -= 50;
+        yAngle = -90 + yAngle;
+        bAngle = -90 + bAngle;
+        //Now cast angles to [0, 359] domain angle flip them
+        yAngle = (yAngle + 360) % 360;
+        bAngle = (bAngle + 360) % 360;
 
-        //Now calculate angles and distance
-        yAngle = atan2(true_yy, true_xy) * 180 / 3.14;
-        bAngle = atan2(true_yb, true_xb) * 180 / 3.14;
         yAngleFix = yAngle - compass->getValue() ;
         bAngleFix = bAngle - compass->getValue() ;
 
-        yDist = sqrt( true_yy*true_yy  + true_xy*true_xy );
+        yDist = sqrt( (50-true_yy)*(50-true_yy) + (50-true_xy)*(50-true_xy) );
-        bDist = sqrt( true_yb*true_yb  + true_xb*true_xb );
+        bDist = sqrt( (50-true_yb)*(50-true_yb) + (50-true_xb)*(50-true_xb) );
       }
       end=true;
       start=false;
@@ -50,62 +51,36 @@ void DataSourceCamera :: readSensor(){
 }
 
 
-void DataSourceCamera :: postProcess(){
+int DataSourceCamera::getValueAtk(bool b){
+  return 0;
 }
-
+int DataSourceCamera::getValueDef(bool b){
-int DataSourceCamera::getValueAtk(bool fixed){
+  return 0;
-  //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
   update();
+    DEBUG.print(bAngle);
+    DEBUG.print(" | ");
+    DEBUG.print(bAngleFix);
+    DEBUG.print(" | ");
+    DEBUG.println(bDist);
+    DEBUG.println(" --- ");
+
     DEBUG.print(yAngle);
     DEBUG.print(" | ");
     DEBUG.print(yAngleFix); 
-    DEBUG.println(" --- ");
-
-    DEBUG.print(bAngle);
     DEBUG.print(" | ");
-    DEBUG.println(bAngleFix); 
+    DEBUG.println(yDist);
     DEBUG.println("---------------");
-    DEBUG.print(true_xb);
+    DEBUG.print(xb);
     DEBUG.print("|");
-    DEBUG.print(true_yb);
+    DEBUG.print(yb);
     DEBUG.print("|");
-    DEBUG.print(true_xy);
+    DEBUG.print(xy);
     DEBUG.print("|");
-    DEBUG.print(true_yy); 
+    DEBUG.println(yy); 
     DEBUG.println("---------------");
-    delay(75);
+    delay(150);
-}
-
-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);
-
-    return d + imuOff;
 }

src/main.cpp

@@ -4,9 +4,9 @@
 #include "games.h"
 
 void setup() {
-  delay(500);
+  delay(1000);
 
-  DEBUG.begin(19600);
+  DEBUG.begin(9600);
   initSensors();
   initGames();
   
@@ -20,8 +20,7 @@ void loop() {
   goalie->play(role==1);
   keeper->play(role==0);  
 
-
+camera->test();
-  camera->test();
   
   // Last thing to do: movement  
   drive->drivePrepared();  

utility/OpenMV/conic_eff.py

@@ -39,8 +39,8 @@ blue_led.on()
 
 
 
-thresholds = [  (70, 100, -18, 21, 21, 63),    # thresholds yellow goal
+thresholds = [  (49, 81, -18, 29, 22, 83),    # thresholds yellow goal
-                (31, 49, -10, 5, -39, -18)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (0, 61, -4, 55, -81, -12)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)
 
@@ -60,11 +60,11 @@ clock = time.clock()'''
 sensor.reset()
 sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QQVGA)
-sensor.set_contrast(+0)
+sensor.set_contrast(+3)
-sensor.set_saturation(+0)
+sensor.set_saturation(+3)
-sensor.set_brightness(0)
+sensor.set_brightness(-1)
 sensor.set_quality(0)
-sensor.set_auto_exposure(False, 3500)
+sensor.set_auto_exposure(False, 5000)
 sensor.set_auto_gain(True)
 sensor.skip_frames(time = 300)
 

utility/ball_read/ball_read/ball_read.ino

@@ -0,0 +1,218 @@
+/**
+  Sensor Mapping
+    Sensor  Angle Pin Port
+    S0      0     A3  C3
+    S1            A2  C2
+    S2            A1  C1
+    S3            A0  C0
+    S4      90    13  B5
+    S5            12  B4
+    S6            11  B3
+    S7            10  B2
+    S8      180   9   B1
+    S9            8   B0
+    S10           7   D7
+    S11           6   D6
+    S12     270   5   D5
+    S13           4   D4
+    S14           3   D3
+    S15           2   D2
+    loop cycle duration: 3.2 millis
+**/
+
+#define S9 ((PINB & 1))
+#define S8 ((PINB & 2) >> 1)
+#define S7 ((PINB & 4) >> 2)
+#define S6 ((PINB & 8) >> 3)
+#define S5 ((PINB & 16) >> 4)
+#define S4 ((PINB & 32) >> 5)
+
+#define S3 ((PINC & 1))
+#define S2 ((PINC & 2) >> 1)
+#define S1 ((PINC & 4) >> 2)
+#define S0 ((PINC & 8) >> 3)
+
+#define S15 ((PIND & 4) >> 2)
+#define S14 ((PIND & 8) >> 3)
+#define S13 ((PIND & 16) >> 4)
+#define S12 ((PIND & 32) >> 5)
+#define S11 ((PIND & 64) >> 6)
+#define S10 ((PIND & 128) >> 7)
+
+#define NCYCLES 350
+#define BROKEN  300
+#define TOO_LOW 60
+
+int counter[16];
+int pins[] = {A3, A2, A1, A0, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2};
+int distance;
+int nmax = 0;
+int sensor = 0;
+
+int oldIndex = 0;
+int oldDistance = 0;
+
+byte ballInfo = 0;
+
+float xs[16];
+float ys[16];
+
+float angle = 0, dist = 0;
+boolean sending = false;
+byte sendAngle = 0, sendDistance = 0;
+byte sendByte = 0;
+
+unsigned long t = 0;
+
+void setup() {
+  delay(1000);
+
+  Serial.begin(57600);
+
+  pinMode(2, INPUT);
+  pinMode(3, INPUT);
+  pinMode(4, INPUT);
+  pinMode(5, INPUT);
+  pinMode(6, INPUT);
+  pinMode(7, INPUT);
+  pinMode(8, INPUT);
+  pinMode(9, INPUT);
+  pinMode(10, INPUT);
+  pinMode(11, INPUT);
+  pinMode(12, INPUT);
+  pinMode(13, INPUT);
+  pinMode(A0, INPUT);
+  pinMode(A1, INPUT);
+  pinMode(A2, INPUT);
+  pinMode(A3, INPUT);
+
+  pinMode(A4, OUTPUT);
+
+  for (int i = 0; i < 16; i++) {
+    xs[i] = cos((22.5 * PI / 180) * i);
+    ys[i] = sin((22.5 * PI / 180) * i);
+  }
+}
+
+void loop() {
+  readBallInterpolation();
+  sendDataInterpolation();
+}
+
+/**--- READ BALL USING SENSORS ANGLE INTERPOLATION ---**/
+
+void readBallInterpolation() {
+  for (int i = 0; i < 16; i++) {
+    counter[i] = 0;
+  }
+
+  //reads from the register
+  for (int i = 0; i < NCYCLES; i++) {
+    counter[0] += !S0;
+    counter[1] += !S1;
+    counter[2] += !S2;
+    counter[3] += !S3;
+    counter[4] += !S4;
+    counter[5] += !S5;
+    counter[6] += !S6;
+    counter[7] += !S7;
+    counter[8] += !S8;
+    counter[9] += !S9;
+    counter[10] += !S10;
+    counter[11] += !S11;
+    counter[12] += !S12;
+    counter[13] += !S13;
+    counter[14] += !S14;
+    counter[15] += !S15;
+  }
+
+  float x = 0, y = 0;
+  for (int i = 0; i < 16; i++) {
+    if (counter[i] > BROKEN || counter[i] < TOO_LOW) counter[i] = 0;
+
+    x += xs[i] * counter[i];
+    y += ys[i] * counter[i];
+  }
+
+  angle = atan2(y, x) * 180 / PI;
+  angle = ((int)(angle + 360)) % 360;
+
+  //distance is 0 when not seeing ball
+  //dist = hypot(x, y);
+  
+  nmax = 0;
+  //saves max value and sensor
+  for (int i = 0; i < 16; i++) {
+    if (counter[i] > nmax) {
+      nmax = counter[i];
+    }
+  }
+
+  dist = nmax;
+  
+  //turn led on
+  if (dist == 0) {
+    digitalWrite(A4, LOW);
+  } else {
+    digitalWrite(A4, HIGH);
+  }
+}
+
+void sendDataInterpolation() {
+  if(sending){
+    sendAngle = ((byte) (angle / 2)) & 0b11111110;
+    Serial.write(sendAngle);
+  }else{
+    sendDistance = dist;
+    if(sendDistance > 254) sendDistance = 254;
+    sendDistance = sendDistance |= 0b00000001;
+    Serial.write(sendDistance);
+  }
+  sending = !sending;
+}
+
+void test() {
+  readBallInterpolation();
+
+  Serial.print(S0);
+  Serial.print(" | ");
+  Serial.print(S1);
+  Serial.print(" | ");
+  Serial.print(S2);
+  Serial.print(" | ");
+  Serial.print(S3);
+  Serial.print(" | ");
+  Serial.print(S4);
+  Serial.print(" | ");
+  Serial.print(S5);
+  Serial.print(" | ");
+  Serial.print(S6);
+  Serial.print(" | ");
+  Serial.print(S7);
+  Serial.print(" | ");
+  Serial.print(S8);
+  Serial.print(" | ");
+  Serial.print(S9);
+  Serial.print(" | ");
+  Serial.print(S10);
+  Serial.print(" | ");
+  Serial.print(S11);
+  Serial.print(" | ");
+  Serial.print(S12);
+  Serial.print(" | ");
+  Serial.print(S13);
+  Serial.print(" | ");
+  Serial.print(S14);
+  Serial.print(" | ");
+  Serial.print(S15);
+  Serial.print(" ()  ");
+  Serial.println(sensor);
+}
+
+void printCounter() {
+  for (int i = 0; i < 16; i++) {
+    Serial.print(counter[i]);
+    Serial.print(" | ");
+  }
+  Serial.println();
+}

utility/bluetooth/bluesmirf-rn42/bluesmirf-rn42.ino

@@ -0,0 +1,56 @@
+/*
+  Example Bluetooth Serial Passthrough Sketch
+  by: Jim Lindblom
+  SparkFun Electronics
+  date: February 26, 2013
+  license: Public domain
+
+  This example sketch converts an RN-42 bluetooth module to
+  communicate at 9600 bps (from 115200), and passes any serial
+  data between Serial Monitor and bluetooth module.
+*/
+#include <SoftwareSerial.h>
+
+int bluetoothTx = 2;  // TX-O pin of bluetooth mate, Arduino D2
+int bluetoothRx = 3;  // RX-I pin of bluetooth mate, Arduino D3
+
+//SoftwareSerial bluetooth(bluetoothTx, bluetoothRx);
+<<<<<<< HEAD
+=======
+
+#define bluetooth Serial3
+>>>>>>> 5c0171dd181357ae6f20ec98cb5c060f067c30fc
+
+#define bluetooth Serial3
+
+void setup() {
+  Serial.begin(9600);  // Begin the serial monitor at 9600bps
+<<<<<<< HEAD
+  
+  bluetooth.begin(9600);  // Start bluetooth serial at 9600
+=======
+  bluetooth.begin(9600);
+>>>>>>> 5c0171dd181357ae6f20ec98cb5c060f067c30fc
+}
+
+void read() {
+  while (bluetooth.available()) {
+    // Send any characters the bluetooth prints to the serial monitor
+    Serial.print((char)bluetooth.read());
+    delay(50);
+  }
+}
+
+void write() {
+  while (Serial.available()) {
+    // Send any characters the Serial monitor prints to the bluetooth
+    bluetooth.print((char)Serial.read());
+    delay(50);
+  }
+  // and loop forever and ever!
+}
+
+void loop() {
+  read();
+  write();
+}

utility/bluetooth/bluesmirf-rn42_2/bluesmirf-rn42_2.ino

@@ -0,0 +1,68 @@
+/*
+  Example Bluetooth Serial Passthrough Sketch
+  by: Jim Lindblom
+  SparkFun Electronics
+  date: February 26, 2013
+  license: Public domain
+
+  This example sketch converts an RN-42 bluetooth module to
+  communicate at 9600 bps (from 115200), and passes any serial
+  data between Serial Monitor and bluetooth module.
+*/
+#include <SoftwareSerial.h>
+
+int bluetoothTx = 2;  // TX-O pin of bluetooth mate, Arduino D2
+int bluetoothRx = 3;  // RX-I pin of bluetooth mate, Arduino D3
+
+SoftwareSerial bluetooth(bluetoothTx, bluetoothRx);
+
+void setup()
+{
+  Serial.begin(9600);  // Begin the serial monitor at 9600bps
+
+  /*bluetooth.begin(115200);  // The Bluetooth Mate defaults to 115200bps
+    bluetooth.print("$");  // Print three times individually
+    bluetooth.print("$");
+    bluetooth.print("$");  // Enter command mode
+    delay(100);  // Short delay, wait for the Mate to send back CMD
+    bluetooth.println("U,9600,N");  // Temporarily Change the baudrate to 9600, no parity
+    //115200 can be too fast at times for NewSoftSerial to relay the data reliably*/
+
+  delay(1000);
+
+  bluetooth.begin(9600);  // Start bluetooth serial at 9600
+  bluetooth.print("$");  // Print three times individually
+  bluetooth.print("$");
+  bluetooth.print("$");
+  read();
+  delay(1000);
+  bluetooth.println("SA,0");
+  read();
+  bluetooth.println("SM,6");
+  read();
+  delay(1000);
+  bluetooth.println("C");
+  read();
+}
+
+void read() {
+  while (bluetooth.available()) {
+    // Send any characters the bluetooth prints to the serial monitor
+    Serial.print((char)bluetooth.read());
+    delay(50);
+  }
+}
+
+void write() {
+  while (Serial.available()) {
+    // Send any characters the Serial monitor prints to the bluetooth
+    bluetooth.print((char)Serial.read());
+    delay(50);
+  }
+  // and loop forever and ever!
+}
+
+void loop() {
+  bluetooth.write(42);
+  delay(500);
+}

utility/bluetooth/reset_bluesmirf-rn42/reset_bluesmirf-rn42.ino

@@ -0,0 +1,41 @@
+#include <SoftwareSerial.h>
+
+int bluetoothTx = 2;  // TX-O pin of bluetooth mate, Arduino D2
+int bluetoothRx = 3;  // RX-I pin of bluetooth mate, Arduino D3
+
+SoftwareSerial bluetooth(bluetoothTx, bluetoothRx);
+void setup() {
+  Serial.begin(9600);
+  Serial.print("$");
+  Serial.print("$");
+  Serial.print("$");
+  delay(1000);
+  Serial.println("SF,1");
+  delay(500);
+  Serial.println("R,1");
+  delay(2500);
+  Serial.println("---");
+  //bluetooth.begin(9600);
+}
+
+void read() {
+  if (bluetooth.available()) {
+    // Send any characters the bluetooth prints to the serial monitor
+    Serial.print((char)bluetooth.read());
+    delay(75);
+  }
+}
+
+void write() {
+  if (Serial.available()) {
+    // Send any characters the Serial monitor prints to the bluetooth
+    bluetooth.print((char)Serial.read());
+    delay(75);
+  }
+  // and loop forever and ever!
+}
+
+void loop() {
+  read();
+  write();
+}

utility/srf02_test_reprogram/srf02_test_reprogram.ino

@@ -0,0 +1,69 @@
+#include <Wire.h>
+
+void setup() {
+  Wire.begin();                // join i2c bus (address optional for master)
+  Serial.begin(9600);          // start serial communication at 9600bps
+
+  changeAddress(114, 230);
+}
+
+int reading = 0;
+
+void loop() {
+  // step 1: instruct sensor to read echoes
+  Wire.beginTransmission(115); // transmit to device #115 (0x70)
+  // the address specified in the datasheet is 224 (0xE0)
+  // but i2c adressing uses the high 7 bits so it's 115
+  Wire.write(byte(0x00));      // sets register pointer to the command register (0x00)
+  Wire.write(byte(0x51));      // command sensor to measure in "inches" (0x50)
+  // use 0x51 for centimeters
+  // use 0x52 for ping microseconds
+  Wire.endTransmission();      // stop transmitting
+
+  // step 2: wait for readings to happen
+  delay(70);                   // datasheet suggests at least 65 milliseconds
+
+  // step 3: instruct sensor to return a particular echo reading
+  Wire.beginTransmission(115); // transmit to device #115
+  Wire.write(byte(0x02));      // sets register pointer to echo #1 register (0x02)
+  Wire.endTransmission();      // stop transmitting
+
+  // step 4: request reading from sensor
+  Wire.requestFrom(115, 2);    // request 2 bytes from slave device #115
+
+  // step 5: receive reading from sensor
+  if (2 <= Wire.available()) { // if two bytes were received
+    reading = Wire.read();  // receive high byte (overwrites previous reading)
+    reading = reading << 8;    // shift high byte to be high 8 bits
+    reading |= Wire.read(); // receive low byte as lower 8 bits
+    Serial.println(reading);   // print the reading
+  }
+
+  delay(250);                  // wait a bit since people have to read the output :)
+}
+
+// The following code changes the address of a Devantech Ultrasonic Range Finder (SRF10 or SRF08)
+// usage: changeAddress(0x70, 0xE6);
+
+void changeAddress(byte oldAddress, byte newAddress)
+{
+  Wire.beginTransmission(oldAddress);
+  Wire.write(byte(0x00));
+  Wire.write(byte(0xA0));
+  Wire.endTransmission();
+
+  Wire.beginTransmission(oldAddress);
+  Wire.write(byte(0x00));
+  Wire.write(byte(0xAA));
+  Wire.endTransmission();
+
+  Wire.beginTransmission(oldAddress);
+  Wire.write(byte(0x00));
+  Wire.write(byte(0xA5));
+  Wire.endTransmission();
+
+  Wire.beginTransmission(oldAddress);
+  Wire.write(byte(0x00));
+  Wire.write(newAddress);
+  Wire.endTransmission();
+}