|
| Wednesday, January 16, 2008 |
| Software For Robot |
Interactive C:
The development tool for the used for The Handy Board is called Interactive C (IC). IC is basically consist of two parts, a pseudo-code (pcode) machine language module and the interface/compiler portion (IC program).
IC compiles into pseudo-code (p-code) which is then executed by a custom stack machine. The p-code is interpreted by a run-time machines language program store in the 6811 MPU. This unique approach was taken because it allows IC to have run-time error check, small object code, source code portability and finally the most useful and powerful feature, multi tasking. The only drawback is at the expense of slower execution speed. However, this should not be a problem with most applications.
IC supports a subset of C, it includes:
Control structure; for, while, if, else
Local, global variables
Arrays, Pointers
16, 32 bits integers
32 bits floating point numbers
Here is a block diagram of how IC works.
IC Library functions:
DC Motors :
void fd(int m)
Turns motor m on in the forward direction. Example: fd(3);
void bk(int m)
Turns motor m on in the backward direction. Example: bk(1);
void off(int m)
Turns off motor m. Example: off(1);
void alloff(), void ao()
Turns off all motors. ao is a short form for alloff.
void motor(int m, int p)
Turns on motor m at power level p. Power levels range from 100 for full on forward to -100 for full on backward.
Servo Motor :
void servo_on()
Enables PA7 servo output waveform.
void servo_off()
Disables PA7 servo output waveform.
int servo(int period)
Sets length of servo control pulse. Value is the time in half-microseconds of the positive portion of a rectangular wave that is generated on the PA7 pin for use in controlling a servo motor. Minimum allowable value is 1400 (i.e., 700 sec); maximum is 4860.
int servo_rad(float angle)
Sets servo angle in radians.
int servo_deg(float angle)
Sets servo angle in degrees.
Sensor Input:
int digital(int p)
Returns the value of the sensor in sensor port p, as a true/false value (1 for true and 0 for false).
int analog(int p)
Returns value of sensor port numbered p. Result is integer between 0 and 255.
User Buttons and Knob:
int stop_button()
Returns value of button labeled Stop: 1 if pressed and 0 if released.
int start_button()
Returns value of button labeled Start.
void stop_press()
Waits for Stop button to be pressed, then released. Then issues a short beep and returns.
void start_press()
Like stop_press, but for the Start button.
int knob()
Returns the position of a knob as a value from 0 to 255.
Infrared Subsystem:
int sony_init(1)
Enables the infrared driver.
int sony_init(0)
Disables the infrared driver.
int ir_data(int dummy)
Returns the data byte last received by the driver, or zero if no data has been received since the last call.
Time Commands:
void reset_system_time()
Resets the count of system time to zero milliseconds.
long mseconds()
Returns the count of system time in milliseconds.
float seconds()
Returns the count of system time in seconds, as a floating point number. Resolution is one millisecond.
void sleep(float sec)
Waits for an amount of time equal to or slightly greater than sec seconds.
void msleep(long msec)
Waits for an amount of time equal to or greater than msec milliseconds. msec is a long integer.
Tone Functions:
void beep()
Produces a tone of 500 Hertz for a period of 0.3 seconds.
void tone(float frequency, float length)
Produces a tone at pitch frequency Hertz for length seconds. Both frequency and length are floats.
void set_beeper_pitch(float frequency)
Sets the beeper tone to be frequency Hz. The subsequent function is then used to turn the beeper on.
void beeper_on()
Turns on the beeper at last frequency selected by the former function.
void beeper_off()
Turns off the beeper.
Multi-Tasking:
int start_process( function-call( : :):, [TICKS] , [STACK-SIZE] )
start_process returns an integer, which is the process ID assigned to the new process.
int kill_process(int pid) kill_all
kills all currently running processes.
ps
prints out a list of the process status. The following information is presented: process ID, status code, program counter, stack pointer, stack pointer origin, number of ticks, and name of function that is currently executing.
void hog_processor()
Allocates an additional 256 milliseconds of execution to the currently running process.
void defer()
Makes a process swap out immediately after the function is called.
Floating Point Functions:
float sin(float angle)
Returns sine of angle. Angle is specified in radians; result is in radians.
float cos(float angle)
Returns cosine of angle. Angle is specified in radians; result is in radians.
float tan(float angle)
Returns tangent of angle. Angle is specified in radians; result is in radians.
float atan(float angle)
Returns arc tangent of angle. Angle is specified in radians; result is in radians.
float sqrt(float num)
Returns square root of num.
float log10(float num)
Returns logarithm of num to the base 10.
float log(float num)
Returns natural logarithm of num.
float exp10(float num)
Returns 10 to the num power.
float exp(float num)
Returns e to the num power.
(float) a ^ (float) b
Returns a to the b power.
Memory Access Functions:
int peek(int loc)
Returns the byte located at address loc.
int peekword(int loc)
Returns the 16-bit value located at address loc and loc+1. loc has the most significant byte, as per the 6811 16-bit addressing standard.
void poke(int loc, int byte)
Stores the 8-bit value byte at memory address loc.
void pokeword(int loc, int word)
Stores the 16-bit value word at memory addresses loc and loc+1.
void bit_set(int loc, int mask)
Sets bits that are set in mask at memory address loc.
void bit_clear(int loc, int mask)
Clears bits that are set in mask at memory address loc.
Here is an hierarchical sturcture of the IC library functions in relations to user programs.
 Software model
The ellipses are individual tasks. The sensors checking tasks writes the result of its sesnsor test to global variables. The "Main Program" polls these global variables and decides on what to do. It also have to make sure that only one "Move Robot" task can be active at a time. This information is communicated to the "Main Program" via task status global variables.
/*****************************************************
***** Dave's Multi-tasking Sumo Robot Program *****
***** Version .91 May 18, 97 *****
*****************************************************/
/*****************************************************
********** Global Variablies **************
*****************************************************
********** Common fixed values **************
*****************************************************/
int moving=1,
not_moving=0,
robot_status,
running=1,
not_running=0;
/*****************************************************
********** Analog ports *************
*****************************************************
*********** Infared edge sensors ***************
*****************************************************
****** Values for edge ******
****** 1 = fl_edge ******
****** 2 = fr_edge ******
****** 4 = rl_edge ******
****** 8 = rr_edge ******/
int rr_edge=0,
rl_edge=1,
fr_edge=2,
fl_edge=3,
first_edge=0,
last_edge=3,
suface_value=0,
off_edge_value=0,
white_value=0,
temp_edge=0,
edge=0;
/*****************************************************
************** Digital Ports ***************
*****************************************************/
int left_ultra=12,
front_left_ultra=13,
front_right_ultra14,
right_ultra=15,
first_ultra=12,
last_ultra=15,
ultra=0,
temp_ultra=0;
int left_contact=7,
right_contact=8,
/*front_contact=xx,
rear_contact=xx,*/
first_contact=7,
last_contact=8,
contact=0,
temp_contact=0;
/*****************************************************
******** Direction, Speed, and Duration **********
*****************************************************/
int fast=100,
medium=50,
slow=11,
forward=1,
right=2,
left=3,
fast_pivot=100,
slow_pivot=50,
reverse=-1;
long forever=255l;
/*****************************************************
********* Process ID's **************
*****************************************************/
int edge_pid,move_pid,contact_pid,ultra_pid,
edge_evade_pid,contact_evade_pid,tracking_pid;
/*****************************************************
********* Process Status **************
*****************************************************/
int edge_status,move_status,ultra_status,contact_evade_status,
edge_evade_status,tracking_status;
int edge_evade_stage1=not_running;
/*****************************************************
******** Sensor Checking Taskes ***********
*****************************************************
******** Edge detection Analog ports 0-3 ***********
*****************************************************/
void check_edge()
{
int x,e;
while(1)
{
e=0;
for (x=first_edge; x<=last_edge; x++) { e <<= 1; /* shift e 1 bit to the left */ if (analog(x) <= white_value) /*||analog(x) >= off_edge_value)*/
e |= 1; /* OR e with 0001 */
}
/*printf("edge=%d e=%d\n",edge,e);*/
if (e != edge)
edge=e;
}
}
/*****************************************************
****** Contact sensors, Digital ports 7-10 *******
*****************************************************/
void check_contact()
{
int x,c;
while(1)
{
c=0;
for (x=first_contact; x<=last_contact; x++) { c <<= 1; /* shift c 1 bit to the left */ if (digital(x)) c |= 1; /* OR c with 0001 */ } /*printf("contact=%d c=%d\n",contact,c);*/ if (c != contact) contact=c; } } /***************************************************** ****** Obstacle detection Digital ports 12-15 ******* *****************************************************/ void check_ultra() { int u,x,y,z; while (1) { u=0; for(x=first_ultra; x<=last_ultra; x++) /* left to right */ { u <<= 1; /* shift u 1 bit to the left */ if(digital(x)) u |= 1; /* OR u with 0001 */ } printf("ultra=%d u=%d\n",ultra,u); if (u != ultra) ultra=u; } } /***************************************************** ********* Move robot ************** *****************************************************/ void move(int direction, int speed, long duration) { int sp1,sp2; long dur; move_status=running; if(direction==forward||direction==reverse) { sp1=direction*speed; sp2=sp1; } else if(direction==right) { sp1=reverse*speed; sp2=speed; } else if(direction==left) { sp1=speed; sp2=reverse*speed; } robot_status=moving; motor(0,sp1); motor(1,sp2); motor(2,sp2); motor(3,sp1); if (duration!=forever) { dur=mseconds(); while(duration >= mseconds()-dur);
ao();
robot_status=not_moving;
}
move_status=not_running;
kill_process(move_pid);
}
/*****************************************************
************ Edge Evasive Maneuver ***************
*****************************************************/
void edge_evade()
{
int dir,temp;
edge_evade_status=running;
temp=temp_edge;
/********** move away from edge ***********/
/* front edge detectors dir=reverse */
/* rear edge detectors dir=forward */
if (temp==1||temp==2||temp==3)
dir=reverse;
else if (temp==4||temp==8||temp==12)
dir=forward;
edge_evade_stage1=running;
robot_status=moving;
move_pid=start_process(move(dir,fast,forever));
while (robot_status==moving);
edge_evade_stage1=not_running;
/****** turns towards the centre of platform ******/
if (temp==2||temp==8)
dir=left;
else if(temp==1||temp==4)
dir=right;
msleep(300l);
move_status=running;
move_pid=start_process(move(dir,medium,1500l));
while (move_status==running);
/******* move towards centre of platform **********/
move_status=running;
move_pid=start_process(move(forward,slow,750l));
while(move_status==running);
edge_evade_status=not_running;
kill_process(edge_evade_pid);
}
/*****************************************************
********** Contact Evasive Maneuver **************
*****************************************************/
void contact_evade()
{
int c_dir;
contact_evade_status=running;
/* left contact c_dir=right speed=fast_pivot duration=500l */
/* right contact c_dir=left speed=fast_pivot duration=500l */
/* front contact c_dir=forward speed=fast duration=forever */
/* rear contact c_dir=reverse speed=fast duration=forever */
if (temp_contact==1) /* right contact */
c_dir=left;
else if (temp_contact==2) /* left contact */
c_dir=right;
robot_status=moving;
move_status=running;
if (temp_contact <= 2 && temp_contact !=0) move_pid=start_process(move(c_dir,fast,forever)); while(contact==temp_contact); contact_evade_status=not_running; kill_process(contact_evade_pid); } /***************************************************** ************ Obsticle Tracking *************** *****************************************************/ void tracking() { tracking_status=running; if(temp_ultra==1) move_pid=start_process(move(right,slow,forever)); if(temp_ultra==2||temp_ultra==3||temp_ultra==4||temp_ultra==6||temp_ultra==12) move_pid=start_process(move(forward,slow,forever)); if(temp_ultra==8) move_pid=start_process(move(left,slow_pivot,forever)); tracking_status=not_running; kill_process(tracking_pid); } /***************************************************** ***************************************************** ***************************************************** ***************************************************** ***************************************************** ************ Main Program *************** ***************************************************** ***************************************************** ***************************************************** ***************************************************** ***************************************************** *****************************************************/ void main() { int e,m,k,x,y,z; ao(); /* make sure that all motors and relays are off */ k=1; /* Initalize edge sensor values */ for(x=first_edge; x<=last_edge; x++) { z=0; for(y=0; y<=10; y++) z=z+analog(x); z=z/10; suface_value=suface_value+z; k++; } suface_value=suface_value/k; white_value=suface_value-20; if (suface_value+5 > 253)
off_edge_value=253;
else
off_edge_value=suface_value+5;
edge_pid=start_process(check_edge());
contact_pid=start_process(check_contact());
ultra_pid=start_process(check_ultra());
/* Main loop */
while (1)
{
if(edge!=temp_edge)
{
if(edge!=0)
{
if(edge_evade_status==running)
{
kill_process(edge_evade_pid);
edge_evade_status=not_running;
}
if(contact_evade_status==running)
{
kill_process(contact_evade_pid);
contact_evade_status==not_running;
}
if(tracking_status==running)
{
kill_process(tracking_pid);
tracking_status=not_running;
}
if(move_status==running)
{
kill_process(move_pid);
move_status=not_running;
}
ao();
robot_status=not_moving;
temp_edge=edge;
edge_evade_pid=start_process(edge_evade());
}
else
{
if(edge_evade_stage1==running&&robot_status==moving)
{
ao();
robot_status=not_moving;
if (move_status==running)
kill_process(move_pid);
}
temp_edge=edge;
}
}
else if(contact!=temp_contact&&edge_evade_status==not_running)
{
if(contact!=0)
{
if(tracking_status==running)
{
kill_process(tracking_pid);
tracking_status=not_running;
}
if(move_status==running)
{
kill_process(move_pid);
move_status=not_running;
}
ao();
robot_status=not_moving;
temp_contact=contact;
contact_evade_pid=start_process(contact_evade());
}
else if(contact_evade_status==running || robot_status==moving)
{
if (contact_evade_status==running)
{
kill_process(contact_evade_pid);
contact_evade_status=not_running;
}
ao();
robot_status=not_moving;
temp_contact=contact;
}
}
else if(ultra!=temp_ultra && edge_evade_status==not_running &&
contact_evade_status==not_running)
{
if(ultra!=0)
{
if(tracking_status==running)
{
kill_process(tracking_pid);
tracking_status=not_running;
}
if(move_status==running)
{
kill_process(move_pid);
move_status=not_running;
}
if(robot_status==moving)
{
ao();
robot_status=not_moving;
}
temp_ultra=ultra;
tracking_pid=start_process(tracking());
}
else
{
if(tracking_status==running)
kill_process(tracking_pid);
if(move_status==running)
kill_process(move_pid);
ao();
robot_status=not_moving;
temp_ultra=ultra;
}
}
}
}Labels: Software for robot |
posted by Sachin Maurya @ 8:02 AM  |
|
| 1 Comments: |
-
Hi,
My name is Pratik; I'm a Web Associate for ThomasNet.com. I came across your site and I notice you make mention of great articles.
ThomasNet recently launched a large information base at thomasnet.com/articles, and we have a specific article(s) that I thought you could make use of.
If you have a moment, could you please review the article and see if it's worthy of a mention on your site as an additional resource for your users?
Thanks so much for your time. Hope to hear soon from your end.
Best Regards,
Pratik.
pmaru@thomasnet.com
|
| |
| << Home |
| |
|
|
Share It:
 
 
|
| About Me |
|
![]()
Name: Sachin Maurya
Home: Lucknow, Uttar Pradesh, India
About Me: born thinker
See my complete profile
|
| Previous Post |
|
| Archives |
|
| Links |
|
|
| Robot Images |
|
|
Hi,
My name is Pratik; I'm a Web Associate for ThomasNet.com. I came across your site and I notice you make mention of great articles.
ThomasNet recently launched a large information base at thomasnet.com/articles, and we have a specific article(s) that I thought you could make use of.
If you have a moment, could you please review the article and see if it's worthy of a mention on your site as an additional resource for your users?
Thanks so much for your time. Hope to hear soon from your end.
Best Regards,
Pratik.
pmaru@thomasnet.com