# Mapping Frames against a Servo's Angles [Firefighter Robot]

Might see me often the next few days, sorry. if this is too much to read, Ill come back and edit it down, thought it best to be thorough. Skip down to “the problem” for the explanation. Thank you in advance. Here’s the actual question:

How do I sweep a turret left and right looking for Pixy blocks, map where these blocks are (mapping frame coordinates to servoangle), and then later, accurately aim the turret at those blocks?

Idea: Create a mock firefighter turret that searches for lightsources and high temperatures in a small area, aim at them, and “fire” (beep a few times).

Design: I have a pixy2 sat on top of a turret. Directly underneath the turret is a flame sensor, made to detect change in temp. The Pixy’s signature 1 is set up to only take in extreme sources of light like a lightbulb or a lighter and the room the pixy is in, when in operation, has no other sources of light available than two lightbulbs for testing. The turret can pan and tilt. The pan/tilt control and all other functions are handled by an Arduino Nano Every and not the Pixy’s servo headers.

Function: What the Pixy should do is first, before anything else, come to “rest” where the pan and tilt servos center themselves parallell to the ground.
Next, the Pixy will take in whatever blocks are present at the “Rest” orientation (may be a light bulb, may be nothing and thus no blocks to process) and save the m_x and m_y of whatever blocks are present to the first element in an array of structs.
Next, the turret will pan left and right looking for more lights. It will pan left and right in steps of 25 degrees between limits 140 degrees (“left”) and 40 degrees (“right”) with 90 degrees being “rest”.
Next, for each 25 degree step, the pixy will see if blocks are avail. If so, the Arduino will use the map function to map the m_x and m_y of the block[s] to the current angle of the pan servo in relation to the difference between the limits from above which is 100 degrees, so ±50 degrees either side of the whereever the pan servo currently is. Thats the problem.

The Problem: That drawing shows my thought process. The shaded areas, first off, can be ignored: they show blocks that will be ignored becauuse the block is outside the 140/40 degree limit range. The idea is that for example, with the frame marked “A” in the picture, the Pixy should map the lightsource to 190/90 degrees servo centered @ 140 deg., the same way it would map the lightsource if it was directly in front of the turret and the ligihtsource was 140/40 degrees servo centered @ 90 deg. And this mapped value will give me a position that after the turret is done panning and the Pixy is done looking for more lightsources, the turrent can accurately turn back to this angle and “shoot water at the fire”. But I’m getting random values for the x map and the y map is the same every time I run the code. *Also, the Arduino will check in an array of structs if the servoangle of the center of the block is already present in the one of the structs, within ±10 deg. in order to dispose of duplicates. This seems to be a second problem I may reach out for in a second forum post or maybe on a different forum since it’s related to comparing elements in an array of structs and not the PixyCam.

So that this post doesnt become twice as long, Im posting the relevant code but if anyone wants to see the full code Ill provide it immediatly:

```// Pre-processor Directives
#include <Servo.h>
#include <Pixy2SPI_SS.h>
//The Arduino Helpers Library [2]
#include <Arduino_Helpers.h>
#include <AH/STL/algorithm>
#include <AH/STL/iterator>

// Class/object declarations
Servo recoil_servo; //Create servo objects.
Servo pan_servo;
Servo tilt_servo;
Pixy2SPI_SS pixy; //Create PixyCam2 object using the SPI/Arduino interface.

struct light_type{
int x;
int y;
int frame_angle_x;
int angle_x;
int angle_y;
int temp;
};

light_type light[3];

```

```void record_xy_location(){
Serial.println("+++++In record_xy_location...");
if(pixy.ccc.numBlocks == 0){
Serial.println("There are no blocks in this frame.");
return;
}

for(int l = 0;l<pixy.ccc.numBlocks;l++){
Serial.print("We are dealing with block# ");Serial.print(l+1);Serial.print(" of ");Serial.println(pixy.ccc.numBlocks);
Serial.print("pixy.ccc.blocks[l].m_x = ");Serial.println(pixy.ccc.blocks[l].m_x);
Serial.print("pixy.ccc.blocks[l].m_y = ");Serial.println(pixy.ccc.blocks[l].m_y);
Serial.print("Mapped angles of light = (j,k)(x,y)");Serial.print(j);Serial.print(" , ");Serial.println(k);
//beta3
if(j < pan_limit_1 || j > pan_limit_2 ){
//https://stackoverflow.com/questions/6302227/how-to-break-out-of-a-function/6302235
Serial.println("J is out of bounds of delta(pan_limit_1,pan_limit_2)");
return;
}
if(k < tilt_limit_1 || k > tilt_limit_2 ){
//https://stackoverflow.com/questions/6302227/how-to-break-out-of-a-function/6302235
Serial.println("k is out of bounds of delta(tilt_limit_1,tilt_limit_2)");
return;
}

//beta2
for (int i = 0; i<3; i++){
if(light[i].angle_x!=0){
if((j  < (light[i].angle_x + 20)) || (j > (light[i].angle_x - 20))){
newVal=false;
}
else{
newVal=true;
}
}
else{
isEmpty = i;
Serial.println("For loop broken");
break; //this is an empty spot (equal to 0), theres no more need to compare. We can assume all values after this spot are empty too. break the for loop.
}

if (newVal=true){
light[isEmpty].x=pixy.ccc.blocks[l].m_x;
light[isEmpty].y=pixy.ccc.blocks[l].m_y;
light[isEmpty].angle_x=j;
light[isEmpty].angle_y=k;
newVal=false;
}
}
}

//Debugging prints
Serial.println("After recording xy is complete:");
for(int i = 0; i<3; i++){
Serial.print("i = ");Serial.print(i);
Serial.print(" x = ");Serial.print(light[i].x);
Serial.print(" y = ");Serial.print(light[i].y);
Serial.print(" frame_angle_x = ");Serial.print(light[i].frame_angle_x);
Serial.print(" angle_x = ");Serial.println(light[i].angle_x);
}
}

```

Hi there, I’m having trouble figuring out how to help. Could you state what you need in a sentence or two?

Thanks,
Jesse

Yeah I didn’t explain myself very well,

Basically I want a turret to sweep back and forth looking for light sources in a room. I want the Arduino to save the the location of the light sources and make the servos aim at each light source and fire a dart at it.

Hello,
What you are doing sounds fun and interesting! I guess the first step is reliably detecting the light with Pixy. It may be difficult unless the light is has a specific hue (not white light). Have you been able to reliably detect the light?

Edward

Hello,
It’s been a fun project! I am able to reliably detect the lights; I have two small lamps with high wattage bulbs. The room is dark save for the lights and the lights have no hue.

So working through the problem for the last 5 some odd hours, a lot of my problems were my Arduino code. I solved a lot of the small, non-Pixy issues there. But mapping is still something of a problem and I have a new (or maybe it was always present) where I have a third block being taken in by the arduino when there should just be two. Do you have any idea why additional blocks with different frame data would show up? It might be more than just one, the way my code is set up I think it ignores everything after the third block.

Sometimes the turret will aiim dead center at a light source, but it will never aim at both light sources in one iteration, and it will always aim off into the distance at a blank wall, or inbetween the two light sources.

I went into Pixymon and pushed some of the settings to extremes, like brightness and filtering. By the middle of the night what I saw in pixymon was a pitchblack window with just the two small light sources (as blocks) stark white against the black background. I would show you a scrnshot but I just reflashed the firmware. Having pixymon set this way, I lost any wayward blocks like from reflective surfaces, which looked great in Pixymon. I could hold the turret and swing it back and forth, simulating servo movement, and Pixymon would just show the two light sources and nothing else.

So at this point two issues:
-I may not be mapping the angles correctly (original)
-What I see in Pixymon doesn’t match what I get in Arduino (new)

Steps taken:
-Adjusted Pixymon as stated above (no change)
-Tried various baud rates up to 115200 (no change)
-Had many, many Serial.print statements; removed all of them (no change)
-flashed hardware back to 3.0.11 (previously had been flashed to 3.0.18 after initial purchase)
-Tried regular SPI interface (no response)
-Set Pixy to 1.0 compatibility mode (no response)
-Reduced or removed some of the delays (no change)

One note is that Im using the default Arduino SPI interface in Pixymon, but I’m using the Pixy2SPI_SS library. Because, I dont have the SPI headers on my Arduino Nano Every, unlike the regular Nano. I could never get UART to work or regular SPI. There’s compatibility issues with the Nano Every and timers for older libraries, do you think there could be any issue there? As a last resort I can take it apart and try my UNO.

Final note: While I haven’t tried it in a few days to see if it still functions, I do have a functioning version of the Pan_tilt demo, made from scratch since I’m not using your servo headers. So I know my Pixy is a functioning unit, at least it was at that point in time.

Below is a link to the code, with the serial print statements removed. Thank you for any help!

Test_PixyCam2_SweepTrack_03b/Test_PixyCam2_SweepTrack_03b.ino at main · jfseneca/Test_PixyCam2_SweepTrack_03b (github.com)

EDIT: This forum post sounds similar to what Im trying to achieve and the response there is likely the reason my accuracy is low. Still hoping for any insight, though. Especially w/ that third block popping up.

EDIT2I think what I’m actually asking for… if my googling iis correct, is if its possible to map the pixy’s cartesian frame to either a polar coordinate system or a spherical coordinate system. So that the turret can rotate accurately and aim towards the object in the frame. Which appears to be very math intensive and diffiicult… I wonder if I could get the turret close enough then introduce PID to get the rest of the way there.

Hello,
Regarding the detection accuracy, I would recommend turning down the camera brightness (exposure) such that the only thing that Pixy can see is the light. This way it’s unlikely to falsely detect other objects:

https://docs.pixycam.com/wiki/doku.php?id=wiki:v2:pixymon_index#color-connected-components-tuning-tab

Hope this helps.

Edward