New Programmer working with cmu cam2

[pbrules15]

I am trying to learn programming over the summer so when robotics starts up again, I will have more understanding of the camera.

I have the camera tracking and locking on to the target, but I cannot figure out how to find the angle of the light the camera is looking at. Also, could you tell me what the following means:

mx - The middle of mass x value
my - The middle of mass y value
x1 - The left most corner’s x value
y1 - The left most corner’s y value
x2 - The right most corner’s x value
y2 -The right most corner’s y value

I don't understand what they are trying to say with "left/right most corner's x/y value"

Thank you!

[AdamHeard]

You have to base the camera's angle on the pwm value of the tilt servo. If you use Kevin Watson's code to remap pwm 13-16, you can get muc greater resolution.

[tseres]

mx - The middle of mass x value
my - The middle of mass y value
x1 - The left most corner’s x value
y1 - The left most corner’s y value
x2 - The right most corner’s x value
y2 -The right most corner’s y value

I don't understand what they are trying to say with "left/right most corner's x/y value"

when the camera sees colour to track it, it is just like a giant blob of pixels of the same colour (what it is tracking).

mx=the middle x value of the blob. it's the position on the x axis of the middle of the blob

my=same as above, except on the y axis
x1=the x value of the top(?) left corner
y1=same as above except on the y axis
x2=the x value of the bottom(?) right corner of the blob
y2=same as above except with the y axis

for example, the coordinate of the top left corner of the colour blob would be (x1,y1) and the bottom right corner would be (x2,y2)

hope that helps!

[Astronouth7303]

What the camera is sending is the bounding box of the blob.

x1 - minimum X value
y1 - minimum Y value
x2 - maximum X value
y2 - maximum Y value
mx - median X value (average of x1 and x2, IIRC)
my - median y value (average of y1 and y2)

There is also a confidence value that describes how "full" the bounding box is, IIRC. higher confidence values mean that more of the bounding box is taken up by the blob. You'll have to look up the exact formula for it.

You can also configure the camera to send you all the pixels, but this takes up a lot more time.

[pbrules15]

OK, thanks for the help!

I am using the pixel count to determine the angle. I am also using a mecanum drive. It approaches the light real well, but I am having trouble figuring out which way to go, and when to stop when the robot is squared up with the light. Could anyone take a look at my code and tell me whats going on?


Thanks!






/************************************************** *****************************
* FILE NAME: user_routines_fast.c <FRC VERSION>
*
* DESCRIPTION:
* This file is where the user can add their custom code within the framework
* of the routines below.
*
* USAGE:
* You can either modify this file to fit your needs, or remove it from your
* project and replace it with a modified copy.
*
* OPTIONS: Interrupts are disabled and not used by default.
*
************************************************** *****************************/

#include <stdio.h>

#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
//#include "user_Serialdrv.h"
#include "serial_ports.h"
#include "pwm.h"
#include "camera.h"
#include "tracking.h"
#include "terminal.h"


/*** DEFINE USER VARIABLES AND INITIALIZE THEM HERE ***/

/***
Wait function
***/
short int waitcounter = 0;
void Wait (short int duration)
{
while (duration < waitcounter)
{
waitcounter++;
}
waitcounter = 0;
}

/************************************************** *****************************
* FUNCTION NAME: InterruptVectorLow
* PURPOSE: Low priority interrupt vector
* CALLED FROM: nowhere by default
* ARGUMENTS: none
* RETURNS: void
* DO NOT MODIFY OR DELETE THIS FUNCTION
************************************************** *****************************/
#pragma code InterruptVectorLow = LOW_INT_VECTOR
void InterruptVectorLow (void)
{
_asm
goto InterruptHandlerLow /*jump to interrupt routine*/
_endasm
}


/************************************************** *****************************
* FUNCTION NAME: InterruptHandlerLow
* PURPOSE: Low priority interrupt handler
* If you want to use these external low priority interrupts or any of the
* peripheral interrupts then you must enable them in your initialization
* routine. Innovation First, Inc. will not provide support for using these
* interrupts, so be careful. There is great potential for glitchy code if good
* interrupt programming practices are not followed. Especially read p. 28 of
* the "MPLAB(R) C18 C Compiler User's Guide" for information on context saving.
* CALLED FROM: this file, InterruptVectorLow routine
* ARGUMENTS: none
* RETURNS: void
************************************************** *****************************/
#pragma code
#pragma interruptlow InterruptHandlerLow save=PROD,section(".tmpdata")

void InterruptHandlerLow ()
{
if (PIR1bits.RC1IF && PIE1bits.RC1IE) // rx1 interrupt?
{
#ifdef ENABLE_SERIAL_PORT_ONE_RX
Rx_1_Int_Handler(); // call the rx1 interrupt handler (in serial_ports.c)
#endif
}
else if (PIR3bits.RC2IF && PIE3bits.RC2IE) // rx2 interrupt?
{
#ifdef ENABLE_SERIAL_PORT_TWO_RX
Rx_2_Int_Handler(); // call the rx2 interrupt handler (in serial_ports.c)
#endif
}
else if (PIR1bits.TX1IF && PIE1bits.TX1IE) // tx1 interrupt?
{
#ifdef ENABLE_SERIAL_PORT_ONE_TX
Tx_1_Int_Handler(); // call the tx1 interrupt handler (in serial_ports.c)
#endif
}
else if (PIR3bits.TX2IF && PIE3bits.TX2IE) // tx2 interrupt?
{
#ifdef ENABLE_SERIAL_PORT_TWO_TX
Tx_2_Int_Handler(); // call the tx2 interrupt handler (in serial_ports.c)
#endif
}



// *** IFI Code Starts Here***
//
// unsigned char int_byte;
// if (INTCON3bits.INT2IF && INTCON3bits.INT2IE) /* The INT2 pin is RB2/DIG I/O 1. */
// {
// INTCON3bits.INT2IF = 0;
// }
// else if (INTCON3bits.INT3IF && INTCON3bits.INT3IE) /* The INT3 pin is RB3/DIG I/O 2. */
// {
// INTCON3bits.INT3IF = 0;
// }
// else if (INTCONbits.RBIF && INTCONbits.RBIE) /* DIG I/O 3-6 (RB4, RB5, RB6, or RB7) changed. */
// {
// int_byte = PORTB; /* You must read or write to PORTB */
// INTCONbits.RBIF = 0; /* and clear the interrupt flag */
// } /* to clear the interrupt condition. */
// else
// {
// CheckUartInts(); /* For Dynamic Debug Tool or buffered printf features. */
// }
}


/************************************************** *****************************
* FUNCTION NAME: User_Autonomous_Code
* PURPOSE: Execute user's code during autonomous robot operation.
* You should modify this routine by adding code which you wish to run in
* autonomous mode. It will be executed every program loop, and not
* wait for or use any data from the Operator Interface.
* CALLED FROM: main.c file, main() routine when in Autonomous mode
* ARGUMENTS: none
* RETURNS: void
************************************************** *****************************/
short int autoshutoffcounter = 0;
short int donecounting = 0;
short int rampup = 127;
short int donetraveling = 0;
short int donestraifing = 0;
short int stopdir1 = 0;
short int stopdir2 = 0;
char curpixnum = 0;
char prevpixnum = 0;
char maxpixnum = 0;
char testmove = 0;
#define left 1
#define right 0
char currentdir = 0;

#define fullspeed 40
#define pwmstop 127
const int fullfwd = pwmstop + fullspeed;
const int fullrev = pwmstop - fullspeed;

void User_Autonomous_Code(void)
{
/* Initialize all PWMs and Relays when entering Autonomous mode, or else it
will be stuck with the last values mapped from the joysticks. Remember,
even when Disabled it is reading inputs from the Operator Interface.
*/
pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;
relay1_fwd = relay1_rev = relay2_fwd = relay2_rev = 0;
relay3_fwd = relay3_rev = relay4_fwd = relay4_rev = 0;
relay5_fwd = relay5_rev = relay6_fwd = relay6_rev = 0;
relay7_fwd = relay7_rev = relay8_fwd = relay8_rev = 0;

while (autonomous_mode) /* DO NOT CHANGE! */
{
if (statusflag.NEW_SPI_DATA) /* 26.2ms loop area */
{
Getdata(&rxdata); /* DO NOT DELETE, or you will be stuck here forever! */

/* Add your own autonomous code here. */

//////////////////////////////////////////////////////ROBOT SETUP!!!
// DO NOT CHANGE!!!

//REMOTE PROGRAMMING
relay8_fwd = p4_sw_aux2; //WONT WORK B/C IN AUTONOMOUS
//REMOTE PROGRAMMING!!!

//REMOTE SPEED CONTROLLER AUTO SHUTOFF

//defaultspeedreset += 1; //Sets default
//if (autospeedreset == 1)
// {
// autoshutoffcounter = 0;
// }
if (donecounting == 0) //if not done initializing
{
if (autoshutoffcounter < 40) //1 second = app. 40 counts of program 25
//leaves relay off untill equal to 25 and adds one
{
relay7_fwd = 0;
autoshutoffcounter += 1;
}
else if (autoshutoffcounter == 40) //once it = 25 add one 25
{
relay7_fwd = 1;
autoshutoffcounter += 1;
}
else if (autoshutoffcounter < 52) //once it = 52 count again (wait untill speed controllers are fully initialized
{
autoshutoffcounter += 1;
}
else if (autoshutoffcounter == 52) //if = 52 set donecounting = 1
{
donecounting = 1; //Stops all initialization once done
autoshutoffcounter += 1; //adds one so no other tests will work
Pwm1_green = 1;
}
}
//REMOTE SPEED CONTROLLER AUTO SHUTOFF

//DO NOT CHANGE!!!
//////////////////////////////////////////////////////ROBOT SETUP!!!

// DOESENT WORK!!! NEEDS TO WAIT UNTILL SPEED CONTROLLERS ARE DONE INITIALIZING APP 1.5 SECONDS
// CHECK WAIT FUNCTION

if (donecounting == 1) //Put autonomous code here.//////////really
{

/*pwm03 = 200; would straif in circle
pwm04 = 255-160;
pwm05 = 160;
pwm06 = 255-200;*/

Camera_Handler();
Servo_Track();



if ((T_Packet_Data.pixels > 2) && (donetraveling == 0))
{

if (PAN_SERVO > 134) // camera points right small deadband
{
int rightness = PAN_SERVO - 134;
int rightpwm = fullfwd - ((rightness / 114.0) * fullspeed);

// set left wheels to "full" speed
pwm03 = pwm04 = fullfwd + 10;

// scale right wheel speed to inverse of camera angle
pwm05 = pwm06 = rightpwm;

printf("right... \r");
}
else if (PAN_SERVO < 114) // camera points left
{
int leftness = 114 - PAN_SERVO;
int leftpwm = fullfwd - ((leftness / 114.0) * fullspeed);

// set left wheels to "full" speed
pwm03 = pwm04 = leftpwm;

// scale right wheel speed to inverse of camera angle
pwm05 = pwm06 = fullfwd + 10;

printf("left... \r");
}
else // camera points straight(ish)
{
pwm03 = pwm04 = fullfwd;
pwm05 = pwm06 = fullfwd;

printf("straight... \r");
}
}

if ((TILT_SERVO >= 194) && (donetraveling == 0) && (T_Packet_Data.pixels >= 2)) //was 194
{
pwm03 = 127;
pwm04 = 127;
pwm05 = 127;
pwm06 = 127;
donetraveling = 1;
printf("Stopping... \r");
}





if ((donetraveling == 1) && (donestraifing == 0)) //straif in circle
{
if (testmove = 0) //starts off going left
{
printf("straif in circle... \r");
pwm03 = 200; //left
pwm04 = 255-160;
pwm05 = 160;
pwm06 = 255-200;
currentdir = 1; //sets current dir as 1
testmove = 1; // add one so never do again
prevpixnum = T_Packet_Data.pixels; //sets prev pixle count

waitcounter = 0;
Wait(500000);

printf("Start off left... \r");
}

curpixnum = T_Packet_Data.pixels; //sets current number of pixels

if ((curpixnum < prevpixnum) && (stopdir1 == 0)) //if current number of pixels is less than previous number of pixels
{
stopdir1 = 1;
waitcounter = 0;
Wait(500000);

printf("curpixnum < prevpixnum... \r");
currentdir = !currentdir; //invert direction

if (prevpixnum > maxpixnum) //check if previous pixle number is greater than max pixle number
{

waitcounter = 0;
Wait(500000);

printf("prevpixnum > maxpixnum \r");
maxpixnum = prevpixnum; //set max pixle number to new previous pixle number
}
}

if (curpixnum > prevpixnum) //if values have been increasing
{
printf("curpixnum > prevpixnum \r");

waitcounter = 0;
Wait(500000);

if (curpixnum >= maxpixnum) //if current pixle number is greater than max pixle number
{

waitcounter = 0;
Wait(500000);

printf("curpixnum > maxpixnum \r");
maxpixnum = curpixnum; //set max pixle number to current pixle number
}

else if ((curpixnum < maxpixnum) || (prevpixnum < maxpixnum)) //if max is greater than current (center is passed) invert untill current = max
{
printf("maxpixnum > curpixnum \r");
currentdir = !currentdir; printf("inverting \r"); //invert direction

waitcounter = 0;
Wait(1000);

printf("curpixnum = maxpixnum \r");
pwm03=pwm04=pwm05=pwm06=127; // stop
donestraifing = 1;
}
}

if ((currentdir == 1) && (donestraifing == 0)) //if current dir = 1
{
printf("Straifing left \r");
pwm03 = 180; //left
pwm04 = 255-140;
pwm05 = 140;
pwm06 = 255-180;

waitcounter = 0;
Wait(500000);
}
else if (donestraifing == 0)
{
printf("straifing right \r");
pwm03 = 255-180; //right
pwm04 = 140;
pwm05 = 255-140;
pwm06 = 180;

waitcounter = 0;
Wait(500000);
}

} //end of alignment

if (donestraifing == 1)
{
pwm03=pwm04=pwm05=pwm06=127; // stop
}
prevpixnum = T_Packet_Data.pixels;

//printf("Blob size %d\r", T_Packet_Data.pixels);
//printf("Tilt %d\r", TILT_SERVO);
//printf("x1 %d\r", T_Packet_Data.x1);
//printf("x2 %d\r", T_Packet_Data.x2);
//printf("diff %d\r", T_Packet_Data.x2 - T_Packet_Data.x1);

}

Generate_Pwms(pwm13,pwm14,pwm15,pwm16);

Putdata(&txdata); /* DO NOT DELETE, or you will get no PWM outputs! */
}
}
}

/************************************************** *****************************
* FUNCTION NAME: Process_Data_From_Local_IO
* PURPOSE: Execute user's realtime code.
* You should modify this routine by adding code which you wish to run fast.
* It will be executed every program loop, and not wait for fresh data
* from the Operator Interface.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
************************************************** *****************************/
void Process_Data_From_Local_IO(void)
{
/* Add code here that you want to be executed every program loop. */

}

/************************************************** *****************************
* FUNCTION NAME: Serial_Char_Callback
* PURPOSE: Interrupt handler for the TTL_PORT.
* CALLED FROM: user_SerialDrv.c
* ARGUMENTS:
* Argument Type IO Description
* -------- ---- -- -----------
* data unsigned char I Data received from the TTL_PORT
* RETURNS: void
************************************************** *****************************/

void Serial_Char_Callback(unsigned char data)
{
/* Add code to handle incomming data (remember, interrupts are still active) */
}


/************************************************** ****************************/
/************************************************** ****************************/
/************************************************** ****************************/

[Bomberofdoom]

Any chance you could send us this file? Sorry for being lazy.

[Alan Anderson]

It's very hard to read a significant amount of code when the formatting has been removed. If you have a short section of programming, enclose it in [code] [/code] tags. For complete files, it's usually best to attach it rather than copy it inline.

[tseres]

i'm not trying to be a jerk here, but i can only read code that i have written or else someone has taught me....just my style. when i write my own code, i have to be in the right mindset, and i have my own way of understanding and writing it...porbably like most people....

[Dave Flowerday]

Originally Posted by tseres

i'm not trying to be a jerk here, but i can only read code that i have written or else someone has taught me....just my style. when i write my own code, i have to be in the right mindset, and i have my own way of understanding and writing it...porbably like most people....

Many younger programmers start out that way, however being able to read and understand other people's code (without a big, long explanation from them) is a skill that is an absolute necessity if you plan to pursue a career in software. It is after the first few times that you have to read someone else's code (in order to modify it or fix a bug) when you learn the true value of doing a good job commenting the code you write.

There are very few commercial software products written by a single person. Working with other people's code is just part of the job, so learning how to do it now will benefit you in the long run.

[AdamHeard]

Originally Posted by Dave Flowerday

Many younger programmers start out that way, however being able to read and understand other people's code (without a big, long explanation from them) is a skill that is an absolute necessity if you plan to pursue a career in software. It is after the first few times that you have to read someone else's code (in order to modify it or fix a bug) when you learn the true value of doing a good job commenting the code you write.

There are very few commercial software products written by a single person. Working with other people's code is just part of the job, so learning how to do it now will benefit you in the long run.

Entirely true... I have to debug several extremely old programs at work sometimes... My boss even wrote them, but didn't comment at the time so even he is confused as to how they work.

[tseres]

ya...i just learned how o program after the season ended...i'm a beginner, but i'm learning

[Bomberofdoom]

Still, reading the whole code with an IDE helps figure out a few stuff faster than reading it like it is now.

[steveg]

Also, could you tell us what it does as of right now? It's a lot easier to debug when you know what the desired result vs. actual result is.

[Salik Syed]

I don't see how the number of pixels would give you any positive corelation to angle...
Distance maybe ... although it's sketchy at best.


To figure out if you are aligned look at the PWM value when the camera is facing perfectly straight... this is the value you want to achieve. You can either pan the robot or use the mecanum drive to move the entire base of the camera