void toggleLED() {
// Toggle LED to see when we're sending frames
static uint8_t toggle = 0;
toggle ^= 1;
if(toggle) {
led_setRGB(255, 255, 255);
}
else {
led_setRGB(0, 0, 0);
}
}
void cc_setLED()
{
BlobA *blob;
uint32_t area, color, r, g, b;
uint8_t brightness;
blob = (BlobA *)g_blobs->getMaxBlob();
if (blob)
{
if (blob->m_model<=NUM_MODELS)
color = g_colors[blob->m_model];
else
color = g_colors[0];
area = (blob->m_right - blob->m_left)*(blob->m_bottom - blob->m_top);
brightness = ledBrightness(area);
b = color&0xff;
b = b ? (b*brightness>>8)+1 : 0;
color >>= 8;
g = color&0xff;
g = g ? (g*brightness>>8)+1 : 0;
color >>= 8;
r = color&0xff;
r = r ? (r*brightness>>8)+1 : 0;
led_setRGB(r, g, b);
}
else
// this routine needs to provide good feedback to the user as to whether the camera sees and segments the object correctly.
// The key is to integrate the "growing algorithm" such that the growing algorithm is executed continuously and feedback about
// the "goodness" of the grown region is returned. We're choosing goodness to be some combination of size (because the bigger
// the grown region the better) and saturation (because more saturation the more likely we've found the intended target, vs
// the background, which is typically not saturated.)
// In general with an RGB LED, you can only communicate 2 things--- brightness and hue, so you have 2 dof to play with....
void scaleLED(uint32_t r, uint32_t g, uint32_t b, uint32_t n)
{
uint32_t max, min, current, sat, t;
#if 0 // it seems that green is a little attenuated on this sensor
t = (uint32_t)(G_GAIN*g);
if (t>255)
g = 255;
else
g = t;
#endif
// find min
min = MIN(r, g);
min = MIN(min, b);
// find max
max = MAX(r, g);
max = MAX(max, b);
// subtract min and form sataration from the distance from origin
sat = sqrt((float)((r-min)*(r-min) + (g-min)*(g-min) + (b-min)*(b-min)));
if (sat>30) // limit saturation to preven things from getting too bright
sat = 30;
if (sat<10) // anything less than 15 is pretty uninteresting, no sense in displaying....
current = 0;
else
{
//sat2 = exp(sat/13.0f);
//current = (uint32_t)(SAT_GAIN*sat2) + (uint32_t)(AREA_GAIN*n) + (uint32_t)(SA_GAIN*n*sat2);
current = (uint32_t)(SA_GAIN*n*sat);
}
if (current>LED_MAX_CURRENT/5)
current = LED_MAX_CURRENT/5;
led_setMaxCurrent(current);
#if 0
// find reasonable bias to subtract out
bias = min*75/100;
r -= bias;
g -= bias;
b -= bias;
// saturate
m = 255.0f/(max-bias);
r = (uint8_t)(m*r);
g = (uint8_t)(m*g);
b = (uint8_t)(m*b);
#endif
#if 1
// saturate
rgbUnpack(saturate(rgbPack(r, g, b)), &r, &g, &b);
#endif
//cprintf("r %d g %d b %d min %d max %d sat %d sat2 %d n %d\n", r, g, b, min, max, sat, sat2, n);
led_setRGB(r, g, b);
}
void ButtonMachine::ledPipe()
{
uint8_t r, g, b;
BlobA blob(m_index, (CAM_RES2_WIDTH-BT_CENTER_SIZE)/2, (CAM_RES2_WIDTH+BT_CENTER_SIZE)/2, (CAM_RES2_HEIGHT-BT_CENTER_SIZE)/2, (CAM_RES2_HEIGHT+BT_CENTER_SIZE)/2);
cc_sendBlobs(g_chirpUsb, &blob, 1);
getColor(&r, &g, &b);
saturate(&r, &g, &b);
led_setRGB(r, g, b);
}
void handleButton()
{
static uint32_t btPrev = 0;
uint32_t bt;
static uint8_t r, g, b;
bt = button();
if (bt)
{
cam_getFrame((uint8_t *)SRAM0_LOC, SRAM0_SIZE, 0x21, 0, 0, 320, 200);
getColor(&r, &g, &b);
saturate(&r, &g, &b);
led_setRGB(r, g, b);
}
else if (btPrev)
{
led_setRGB(0, 0, 0);
delayus(50000);
led_setRGB(r, g, b);
delayus(50000);
led_setRGB(0, 0, 0);
delayus(50000);
led_setRGB(r, g, b);
delayus(50000);
led_setRGB(0, 0, 0);
}
btPrev = bt;
}
void edgeDetect_run()
{
cam_setBrightness(BRIGHTNESS); // 0 to 255
uint8_t *frame = (uint8_t *)SRAM1_LOC;
uint8_t *frameloc = (uint8_t *)(SRAM1_LOC + 2);
uint8_t *sendPositions = (uint8_t*)(SRAM1_LOC);
float theta;
// recieve the command to get a frame
while(1) {
led_setRGB(255, 0, 0);
while(1) {
if(UART_DATA_AVAILABLE) {
//rxbuf = UART_ReceiveByte(LPC_USART0);
theta = (float)UART_DATA_AVAILABLE;
UART_DATA_AVAILABLE = 0;
break;
}
}
theta = (float)(theta*(3.14159/180.0));
led_setRGB(0, 255, 0);
cam_getFrame(frameloc, SRAM1_SIZE, CAM_GRAB_M1R2, 0, 0, RES_WIDTH, RES_HEIGHT);
// second time through gets a frame fine
frameloc = frame;
for(uint16_t y = 1 + OFFSET; y < (RES_HEIGHT - OFFSET); y += 2) {
uint16_t ypo = y + 1;
uint16_t ymo = y - 1;
for(uint16_t x = 1 + OFFSET; x < (RES_WIDTH - OFFSET); x += 2) {
uint16_t xpo = x + 1;
uint16_t xmo = x - 1;
uint16_t grad;
uint16_t intense_XPO_Y = frameloc[y*RES_WIDTH + xpo] + frameloc[ypo*RES_WIDTH + xpo+1] +
(frameloc[ypo*RES_WIDTH + xpo] + frameloc[y*RES_WIDTH + xpo+1])/2;
uint16_t intense_XMO_Y = frameloc[y*RES_WIDTH + xmo] + frameloc[ypo*RES_WIDTH + x] +
(frameloc[ypo*RES_WIDTH + xmo] + frameloc[y*RES_WIDTH + x])/2;
uint16_t intense_X_YPO = frameloc[ypo*RES_WIDTH + x] + frameloc[(ypo+1)*RES_WIDTH + xpo] +
(frameloc[(ypo+1)*RES_WIDTH + x] + frameloc[ypo*RES_WIDTH + xpo])/2;
uint16_t intense_XPO_YPO = frameloc[ypo*RES_WIDTH + xpo] + frameloc[(ypo+1)*RES_WIDTH + xpo+1] +
(frameloc[(ypo+1)*RES_WIDTH + xpo] + frameloc[ypo*RES_WIDTH + xpo+1])/2;
uint16_t intense_XMO_YPO = frameloc[(ypo)*RES_WIDTH + xmo] + frameloc[(ypo+1)*RES_WIDTH + x] +
(frameloc[(ypo+1)*RES_WIDTH + xmo] + frameloc[ypo*RES_WIDTH + x])/2;
uint16_t intense_X_YMO = frameloc[ymo*RES_WIDTH + x] + frameloc[y*RES_WIDTH + xpo] +
(frameloc[y*RES_WIDTH + x] + frameloc[ymo*RES_WIDTH + xpo])/2;
uint16_t intense_XPO_YMO = frameloc[ymo*RES_WIDTH + xpo] + frameloc[y*RES_WIDTH + xpo+1] +
(frameloc[y*RES_WIDTH + xpo] + frameloc[ymo*RES_WIDTH + xpo+1])/2;
uint16_t intense_XMO_YMO = frameloc[ymo*RES_WIDTH + xmo] + frameloc[y*RES_WIDTH + x] +
(frameloc[y*RES_WIDTH + xmo] + frameloc[ymo*RES_WIDTH + x])/2;
uint16_t grad1 = abs(intense_XPO_Y - intense_XMO_Y
+ intense_XPO_YPO - intense_XMO_YPO
+ intense_XPO_YMO - intense_XMO_YMO);
uint16_t grad2 = abs(intense_X_YPO - intense_X_YMO
+ intense_XPO_YPO - intense_XPO_YMO
+ intense_XMO_YPO - intense_XMO_YMO);
grad = grad1 + grad2;
// when it loops a second time through, it cannot make it passed the next line
// Deleted if statement, hopefully that'll do 'er. Really inefficient though
if(grad > THREASHOLD) {
frameloc[ymo*RES_WIDTH + xmo] = 255;
frameloc[y*RES_WIDTH + xmo] = 255;
frameloc[ymo*RES_WIDTH + x] = 255;
frameloc[y*RES_WIDTH + x] = 255;
}
else {
frameloc[ymo*RES_WIDTH + xmo] = 0;
frameloc[y*RES_WIDTH + xmo] = 0;
frameloc[ymo*RES_WIDTH + x] = 0;
frameloc[y*RES_WIDTH + x] = 0;
}
/* TEST DIAGONAL LINE
if( y == x) {
frameloc[ymo*RES_WIDTH + xmo] = 255;
frameloc[y*RES_WIDTH + xmo] = 255;
frameloc[ymo*RES_WIDTH + x] = 255;
frameloc[y*RES_WIDTH + x] = 255;
}
else {
frameloc[ymo*RES_WIDTH + xmo] = 0;
frameloc[y*RES_WIDTH + xmo] = 0;
frameloc[ymo*RES_WIDTH + x] = 0;
frameloc[y*RES_WIDTH + x] = 0;
}
*/
}
}
led_setRGB(255, 0, 255);
// floor detection &
uint16_t count = 0;
for(float x = (POS_OFFSET); x < (RES_WIDTH - POS_OFFSET); x += 2.0) {
float xPos;
for(float y = (RES_HEIGHT - POS_OFFSET); y > POS_OFFSET; y -= 2.0) {
if(frameloc[((uint8_t)y)*RES_WIDTH + (uint16_t)x] != 0) {
float yPos;
double theta_ph = atan(((2.0*y-200.0)/200.0)*TAN_FOVH_DIV_2); // This works
double cos_theta_ph = cos(theta_ph);
//sendPositions[count] = cos_theta_ph*128;
double cos_theta_minus_ph = cos(theta - theta_ph);
//sendPositions[count] = cos_theta_minus_ph*128;
//sendPositions[count] = (int8_t)(theta_ph * (180.0/3.142)); // these two lines send x,y pairs
//sendPositions[count] = (cos_theta_ph)/(cos_theta_minus_ph);
yPos = ((double)((3.8)*((cos_theta_ph))))/(cos_theta_minus_ph) +
(0.0)*tan(theta - theta_ph);
xPos = (yPos*(2.0*x - 320.0))/417.0; // hopefully the x cord. won't be super inaccurate. Not crucial though.
//sendPositions[2*count + 1] = yPos; //(int8_t)yPos;
//sendPositions[2*count] = x; // these two lines send x,y pairs
sendPositions[count] = yPos; //(int8_t)yPos;
count++;
break;
}
else {
// color the floor a different color. Not used in this scenario
}
}
}
UART_Send(LPC_USART0, sendPositions, count, BLOCKING); // sends x,y pairs
//UART_Send(LPC_USART0, sendPositions, count, BLOCKING); // sends only the y distance
count = 0;
// Byte packing for processing script
/*
for(uint16_t y = 0; y < RES_HEIGHT/2; y += 1) {
for (uint16_t x = 0; x < RES_WIDTH/2; x += 8) {
*/
/* // Checkerboard for configuring things
if(y&1)
frameloc[y*20 + x/8] = 0x55;
else
frameloc[y*20 + x/8] = 0xAA;
*/
/*
frameloc[y*(RES_WIDTH/16) + x/8] = (frameloc[(y*2+1)*RES_WIDTH + (2*(x+0)+1)] & 0x80) |
(frameloc[(y*2+1)*RES_WIDTH + (2*(x+1)+1)] & 0x40) |
(frameloc[(y*2+1)*RES_WIDTH + (2*(x+2)+1)] & 0x20) |
(frameloc[(y*2+1)*RES_WIDTH + (2*(x+3)+1)] & 0x10) |
(frameloc[(y*2+1)*RES_WIDTH + (2*(x+4)+1)] & 0x08) |
(frameloc[(y*2+1)*RES_WIDTH + (2*(x+5)+1)] & 0x04) |
(frameloc[(y*2+1)*RES_WIDTH + (2*(x+6)+1)] & 0x02) |
(frameloc[(y*2+1)*RES_WIDTH + (2*(x+7)+1)] & 0x01);
}
}
*/
// UART_Send(LPC_USART0, frameloc, 16000, BLOCKING); // Send the frame to see it in tera term. NO BYTE PACK
// frame[0] = 'A'; // key byte
// UART_Send(LPC_USART0, frameloc, 2001, BLOCKING); // Send the frame byte packed to see it in processing
//UART_Send(LPC_USART0, sendPositions, (RES_WIDTH - 2*POS_OFFSET)>>1, BLOCKING);
// clear array
for(uint8_t x = 0; x < 240; x++) {
sendPositions[x] = 255;
}
toggleLED();
}
}
void handleRecv()
{
uint8_t i, a;
static uint16_t w=0xffff;
static uint8_t lastByte;
uint16_t s0, s1;
Iserial *serial = ser_getSerial();
for (i=0; i<10; i++)
{
switch(g_state)
{
case 0: // reset
lastByte = 0xff; // This is not part of any of the sync word most significant bytes
g_state = 1;
break;
case 1: // sync word
if(serial->receive(&a, 1))
{
w = lastByte << 8;
w |= a;
lastByte = a;
g_state = 2; // compare
}
break;
case 2: // receive data byte(s)
if (w==SYNC_SERVO)
{ // read rest of data
if (serial->receiveLen()>=4)
{
serial->receive((uint8_t *)&s0, 2);
serial->receive((uint8_t *)&s1, 2);
//cprintf("servo %d %d\n", s0, s1);
rcs_setPos(0, s0);
rcs_setPos(1, s1);
g_state = 0;
}
}
else if (w==SYNC_CAM_BRIGHTNESS)
{
if(serial->receive(&a, 1))
{
cam_setBrightness(a);
g_state = 0;
}
}
else if (w==SYNC_SET_LED)
{
if (serial->receiveLen()>=3)
{
uint8_t r, g, b;
serial->receive(&r, 1);
serial->receive(&g, 1);
serial->receive(&b, 1);
led_setRGB(r, g, b);
//cprintf("%x %x %x\n", r, g ,b);
g_ledSet = true; // it will stay true until the next power cycle
g_state = 0;
}
}
else
g_state = 1; // try another word, but read only a byte
break;
default:
g_state = 0; // try another whole word
break;
}
}
}
void edgeDetect_highres_run()
{
uint8_t *frame = (uint8_t *)SRAM1_LOC;
uint8_t *frameloc = (uint8_t *)(SRAM1_LOC + 0);
uint8_t *sendPositions = (uint8_t*)(SRAM1_LOC);
float theta;
// recieve the command to get a frame
while(1) {
// red LED: Stopped waiting for data
led_setRGB(255, 0, 0);
while(1) {
if(UART_DATA_AVAILABLE) { // Data has come!
theta = (float)UART_DATA_AVAILABLE;
UART_DATA_AVAILABLE = 0;
break;
}
}
if(theta > 45) {
// make sure that theta is casted as a float
theta = (float)(theta*(3.14159/180.0));
// green LED, lets go!
led_setRGB(0, 255, 0);
// grab frame
cam_getFrame(frameloc, SRAM1_SIZE, CAM_GRAB_M1R2, 0, 0, RES_WIDTH, RES_HEIGHT);
frameloc = frame; //
// double for loop for calculating edges
for(uint16_t y = 1; y < (RES_HEIGHT); y += 1) {
uint16_t ypo = y + 1;
uint16_t ymo = y - 1;
for(uint16_t x = 1; x < (RES_WIDTH); x += 1) {
uint16_t xpo = x + 1;
uint16_t xmo = x - 1;
// Gradient calculation
// intensity calculation for the pixel groups around each pixel
uint16_t intense_XPO_Y;
uint16_t intense_XMO_Y;
uint16_t intense_X_YPO;
uint16_t intense_XPO_YPO;
uint16_t intense_XMO_YPO;
uint16_t intense_X_YMO;
uint16_t intense_XPO_YMO;
uint16_t intense_XMO_YMO;
if( ( (x % 2 == 0) && (y % 2 == 0) ) || ( (x % 2 == 1) && (y % 2 == 1) ) ) { // We are on a blue or red pixel
intense_XPO_Y = intensityCalc_GreenPixel(frameloc, xpo, y);
intense_XMO_Y = intensityCalc_GreenPixel(frameloc, xmo, y);
intense_X_YPO = intensityCalc_GreenPixel(frameloc, x, ypo);
intense_XPO_YPO = intensityCalc_BlueRedPixel(frameloc, xpo, ypo);
intense_XMO_YPO = intensityCalc_BlueRedPixel(frameloc, xmo, ypo);
intense_X_YMO = intensityCalc_GreenPixel(frameloc, x, ymo);
intense_XPO_YMO = intensityCalc_BlueRedPixel(frameloc, xpo, ymo);
intense_XMO_YMO = intensityCalc_BlueRedPixel(frameloc, xmo, ymo);
}
else { // We are on a green pixel
intense_XPO_Y = intensityCalc_BlueRedPixel(frameloc, xpo, y);
intense_XMO_Y = intensityCalc_BlueRedPixel(frameloc, xmo, y);
intense_X_YPO = intensityCalc_BlueRedPixel(frameloc, x, ypo);
intense_XPO_YPO = intensityCalc_GreenPixel(frameloc, xpo, ypo);
intense_XMO_YPO = intensityCalc_GreenPixel(frameloc, xmo, ypo);
intense_X_YMO = intensityCalc_BlueRedPixel(frameloc, x, ymo);
intense_XPO_YMO = intensityCalc_GreenPixel(frameloc, xpo, ymo);
intense_XMO_YMO = intensityCalc_GreenPixel(frameloc, xmo, ymo);
}
float grad1 = abs(intense_XPO_Y - intense_XMO_Y
+ intense_XPO_YPO - intense_XMO_YPO
+ intense_XPO_YMO - intense_XMO_YMO);
float grad2 = abs(intense_X_YPO - intense_X_YMO
+ intense_XPO_YPO - intense_XPO_YMO
+ intense_XMO_YPO - intense_XMO_YMO);
// Threashold detection
if( (grad1 + grad2) > THREASHOLD_LOW ) {
// EDGE
frameloc[y*RES_WIDTH + x] = 255;
}
else {
// NO EDGE
frameloc[y*RES_WIDTH + x] = 0;
}
}
} // end nested for loop
led_setRGB(255, 0, 255); // Purple LED
// floor detection & distance extrapolation
uint16_t count = 0;
for(float x = (POS_OFFSET); x < (RES_WIDTH - POS_OFFSET); x += 1.0) { // start on the left
float xPos;
for(float y = (RES_HEIGHT - POS_OFFSET); y > POS_OFFSET; y -= 1.0) { // start from the bottom
if(frameloc[((uint16_t)y)*RES_WIDTH + (uint16_t)x] != 0) {
float yPos;
double theta_ph = atan(((2.0*y-200.0)/200.0)*TAN_FOVH_DIV_2); // angle of the pixel
double cos_theta_ph = cos(theta_ph); // used in the computations
double cos_theta_minus_ph = cos(theta - theta_ph); // used in the computations
yPos = ((double)((3.9)*((cos_theta_ph))))/(cos_theta_minus_ph) +
(2.1)*tan(theta - theta_ph); // y distance from the bot
xPos = (yPos*(2.0*x - 320.0))/417.0; // x distance from the bot
sendPositions[2*count] = xPos; // these two lines send x,y pairs
sendPositions[2*count + 1] = yPos;
count++; // count of the number of edges of obsticles detected
break; // stop looking for the edge, break to the next x co-ordinate
}
else {
// color the floor a different color. Not used in this scenario
}
}
}
UART_Send(LPC_USART0, sendPositions, 2*count, BLOCKING); // sends x,y pairs
count = 0;
// clear array
for(uint16_t x = 0; x < 640; x++) {
sendPositions[x] = 255;
}
} // end edge detecting
else if(theta > 1 ) { // Servo move routine
uint16_t position;
int8_t retVal;
// Move the servo based on the input from the PIC
// theta == 2 corrisponds to a 45 degree angle,
// theta == 42 corrisponds to a 135 degree angle.
position = (position - 2)*(25);
retVal = rcs_setPos(1, position);
if(retVal == 0) {
uint8_t retStr[] = "successful Servo Move\n\r";
UART_Send(LPC_USART0, retStr, 24, BLOCKING);
}
else {
uint8_t retStr[] = "Error: Servo Move\n\r";
UART_Send(LPC_USART0, retStr, 20, BLOCKING);
}
}
else { // theta == 1, they are asking for my ID
// tell the processing script/pic/whatever that I am the pixy
uint8_t ID[] = "I am the Pixy!\n\r";
UART_Send(LPC_USART0, ID, 17, BLOCKING);
}
}
}
