Stampy Code For Axon problems

[Jdog]

I'm trying to use the edge detection algorithm for the stampy sumo robot on the axon. This is my current code, but For some reason the wheels are not spinning, and my servo just turns all of the way to the right, and doesn't change when I put my hand in front of it. I'm also trying to add a printf function to see exactly what is going on, but it doesn't seem to want to compile with it in there. Can someone help me and see what's wrong with the code?

current code:

Code:

/****************************************************************************
*
*   Copyright (c) 2008 www.societyofrobots.com
*   (please link back if you use this code!)
*
*   This program is free software; you can redistribute it and/or modify
*   it under the terms of the GNU General Public License version 2 as
*   published by the Free Software Foundation.
*
*   Alternatively, this software may be distributed under the terms of BSD
*   license.
*
****************************************************************************/

//Add your code here

//global variables
int sharp_IR_reading=0;

int scan_thresh=30;//threshold value of scanning sensor

int scan_angle=1000;//position of scanner, in units of servo command
int max_scan_angle=833;//maximum position scanner can rotate to (57)
long int wheel_left=600;
long int wheel_right=600;

//this function causes scanning servo to center on edge of object
//automatically calculates threshold value before run
void autocalibrate(void)
{
scan_thresh=a2dConvert8bit(3);//sensor reading
}

void scan(void)
{
//lower (-) goes right
//higher (+) goes left
//30 far right, 50 straight, 56 far left (until it jams)

/*psuedocode
object is detected
scan right object detected
object not detected
scan left until object detected*/

sharp_IR_reading=a2dConvert8bit(3);

if (sharp_IR_reading > scan_thresh)//object detected
{
if (scan_angle>529) //overflow protection
scan_angle-=20;//scan right
}
else //object not detected
{
if (scan_angle<=max_scan_angle) //maximum servo angle
scan_angle+=20; //scan left
//else //if scanned all the way, this forces it to start over
// scan_angle=30;
}

//servo scan code
servo_scan(scan_angle);
}
void control(void)
{
while(1)
{
scan();
//object on left
if(scan_angle < 570)
{//go left
wheel_left=800;
wheel_right=800;
}

//object on right
else if(scan_angle > 830)
{//go right
wheel_left=550;
wheel_right=550;
}

//object is centered
else
{//go straight
wheel_left=800;
wheel_right=550;
}
  }
}

printf code:

Code:

rprintf("Sharp_IR=%d, IR_Servo=%d%d, L_wheel=%d%d, R_wheel=%d%d\r\n",Sharp_Ir_Reading, servo_scan, wheel_left, wheel_right);

[Admin]

Thats because you don't have any code to tell your servos to do anything

For example:
         wheel_left=800;
         wheel_right=550;

declares servo angles, but you don't actually send commands to the servos!

[Jdog]

Yes I do, it's right under void control(void)

[Admin]

Ummmm no you don't . . .

It should be something like this:

Code:

while(1)
{
scan();

//object on left
if(scan_angle > 570)
robot_turn_left();

//object on right
else if(scan_angle < 410)
robot_turn_right();

//object is centered
else
robot_go_straight();

delay_cycles(400);//a small delay to prevent crazy oscillations
}

Where a function such as robot_turn_left(); commands servos.

[Jdog]

Ok I see what your talking about, and I added those in but the wheels still won't turn. This is my code:

Code:

/****************************************************************************
*
*   Copyright (c) 2008 www.societyofrobots.com
*   (please link back if you use this code!)
*
*   This program is free software; you can redistribute it and/or modify
*   it under the terms of the GNU General Public License version 2 as
*   published by the Free Software Foundation.
*
*   Alternatively, this software may be distributed under the terms of BSD
*   license.
*
****************************************************************************/

//Add your code here

//global variables
int sharp_IR=0;

int scan_thresh=30;//threshold value of scanning sensor

int scan_angle=1000;//position of scanner, in units of servo command
int max_scan_angle=527;//maximum position scanner can rotate to (57)
long int wheel_left=600;
long int wheel_right=600;

//this function causes scanning servo to center on edge of object
//automatically calculates threshold value before run
void robot_right(void)
{//go right
wheel_left=550;
wheel_right=550;
}
void robot_left(void)
{
wheel_left=800;
wheel_right=800;
}
void robot_straight(void)
{
wheel_left=800;
wheel_right=550;
}
void autocalibrate(void)
{
scan_thresh=a2dConvert8bit(3);//sensor reading
}

void scan(void)
{
//lower (-) goes left
//higher (+) goes right
//1000 far right, 700 straight, 527 far left (until it jams)

/*psuedocode
object is detected
scan right object detected
object not detected
scan left until object detected*/

sharp_IR=a2dConvert8bit(3);

if (sharp_IR > scan_thresh)//object detected
{
if (scan_angle>529) //overflow protection
scan_angle-=20;//scan left
}
else //object not detected
{
if (scan_angle<=max_scan_angle) //maximum servo angle
scan_angle+=20; //scan right
else
scan_angle=1000;
}

//servo scan code
servo_scan(scan_angle);
}
void control(void)
{
while(1)
{
autocalibrate();
scan();
//object on left
if(scan_angle < 570)
{
robot_left();
}

//object on right
else if(scan_angle > 830)
{
robot_right();
}
//object is centered
else
{
robot_straight();
}
  }
}

[Jdog]

I got my printf function working and I'm confused at what I was getting in hyperterminal. What's with the right wheel and the ir servo?

Sharp_IR=84, IR_Servo=750550, L_Wheel=0550, R_Wheel=08695

Also, It says that that left wheel is getting 550, but the wheel still isn't turning, I tested the servos they're fine, and I made sure they were in the right ports and they are.

[Razor Concepts]

Right now you are only changing the integer values. That isnt actually commanding the servos to move. You have to create a servo (there should be some in hardware.c or something), and then do servo_left(wheel_left); at the end of control.

[Jdog]

I think I already had that, do you mean this:

Code:

#define servo_scan( position )                                                                      servo(PORTE,5,position)
#define wheel_left( position )                                                                       servo(PORTE,6,position)
#define wheel_right( position )                                                                     servo(PORTE,7,position)

[Razor Concepts]

Alright, so in control, at the end of the while loop, do wheel_left(wheel_left); and so on. Then, after that, do delay_ms(20);

[Jdog]

Thank You everybody who answered, I just have a little bit more small modifications, but everything is working almost right. I'll post my final code when I'm done.

[Jdog]

Woohoo after 26 posts and 3 hours of debugging I'm Done!!! Here's the final code.

[Admin]

video, please

Woohoo after 26 posts and 3 hours of debugging I'm Done!!! Here's the final code.

Your first post and last post are 25 hours apart

[Jdog]

video, please

Woohoo after 26 posts and 3 hours of debugging I'm Done!!! Here's the final code.

Your first post and last post are 25 hours apart

Actually it was a little more because I had another post but you said I was getting off topic so I started a new thread.
Anyway here's the video:

http://www.youtube.com/watch?v=33sPmqq4fL4
	LQ | HQ

Also I added my new edited code. I now have it using the autocalibrate function, I added code so that it goes backwards if it gets too close to something and I commented it so that someone who doesn't know much about programming can understand it and hopefully learn from it.

[Admin]

cool, I just added this to the Axon examples page

Got a pic you'd like me to use?

[Jdog]

Yea, This one is good.
I will also upload new pics as I upgrade it.