Please Help! Need help writint a Program for a Pololu 3pi robot in AVR assembly

Question

Please Help! Need help writint a Program for a Pololu 3pi robot in AVR assembly language to solve a circular track as quickly as possible using all 5 sensors offered by the robot. The robot must accept the push button inputs to tell it when to begin and when to stop following the track.

Thank You so much!

This is what I have so far: Using AVR Studio 6.0

.nolist
.include "C:Program FilesAtmelAVR ToolsAvrAssembler2Appnotesm328Pdef.inc"
.list

;===========================
; Declarations

;***** Global register variables
.def   wreg       =R16       ;General use working register
.def   time       =R17       ;Time value passed to subroutine for # of ms
.def   delayReg   =R18       ;used in time loop
.def   CntValue   =R19
.def   temp       =R20
.def   temp2       =R21      
.def   longtime   =R22       ;test register

;***** Other equates
.equ   lcdrs       =2           ;LCD rs pin connected to PD2
.equ   lcdrw       =0           ;LCD r/w pin connected to PB0
.equ   lcde       =4           ;LCD e pin connected to PD4
.equ   line1    =0x80       ;1? ?????? ??? LCD
.equ   line2    =0xC0       ;2? ?????? ??? LCD

;******* Main program entry point on reset

reset:
       ldi       r20,LOW(RAMEND)
       out       SPL,r20               ;Init Stack Pointer
       ldi       r20,HIGH(RAMEND)
       out       SPH,r20               ;Init Stack Pointer
       ldi       r20,0b00111111        ;PB0,1,2,3,4,5 outputs, PB6 & 7 are Xtal
       out       DDRB,r20
       ldi       r20,0b11000000       ;enable internal pull-ups on PB6-PB7
       out       PORTB,r20
       ldi       r20,0b11111111        ;All Port D are outputs
       out       DDRD,r20
       ldi       r20,0b00000000       ;make output lines 0
       out       PORTD,r20
       ldi       r20,0xA1           ;(added)
       out       TCCR0A,r20           ;(added)
       sts       TCCR2A,r20           ;(added)
       ldi       r20,0x01           ;(changed new) (old: set counter for 1024 prescale)
       out       TCCR0B,r20
       sts       TCCR2B,r20           ;(added)
       ldi       time,100           ;delay as LCD powers up
       rcall   delay
       rcall   reset_LCD           ;Reset LCD module
       rcall   lcdinit               ;Initialize LCD module

;===========================================================================
; Main
;===========================================================================
main:   rcall   lcdclr               ;Clear the LCD
       ldi       r16,line1           ;Set address to line 1
       rcall   send_cmd
       ldi       ZH,high(2*message1)   ;Type message1
       ldi       ZL,low(2*message1)   ;to the LCD
       rcall   send_mess
       ldi       r16,line2           ;Set address to line 2
       rcall   send_cmd
       ldi       ZH,high(2*message2)   ;Type message2
       ldi       ZL,low(2*message2)   ;to the LCD
       rcall   send_mess
       rcall   getbutton           ;get a button press to move forward or spin

       rcall   lcdclr               ;Clear the LCD
       ldi       r16,line1           ;Set address to line 1
       rcall   send_cmd
       ldi       ZH,high(2*message3)   ;Type message1
       ldi       ZL,low(2*message3)   ;to the LCD
       rcall   send_mess

end:   rjmp   end

;=========================================================================
;       Reset LCD module
; This is a typical LCD Reset routine. Usually you send a 30, 30, 30, 20
; to the command register, but the LCD on the 3pi uses a 4 bit data interface
; so you can only send 3, 3, 3, 2. After each command sent this routine waits
; about 10ms (1 ms would probably be sufficient, but has not been tested). The
; delay is important to give the LCD time to implement the instruction. You
; could also read the busy line and wait until the LCD is ready, but this is
; a little more complicated - and the 3pi is complicated enough.
;=========================================================================
reset_LCD:
       ldi    r16,0x3
       rcall   send_reset       ; send_reset does not separate the nibbles
       ldi       time,10           ; wait 10ms
       rcall   delay
       ldi    r16,0x3
       rcall   send_reset
       ldi       time,10           ; wait 10ms
       rcall   delay
       ldi    r16,0x3
       rcall   send_reset
       ldi       time,10           ; wait 10ms
       rcall   delay
       ldi    r16,0x2
       rcall   send_reset
       ldi       time,10           ; wait 10ms
       rcall   delay
       ret
;=========================================================================
;       Initialize LCD module
;At this point, the normal 4 bit command routine can be used
;=========================================================================
lcdinit:
       ldi       r16,0x28       ; Function Set: 4-bit, 2 Line, 5x7 Dots
       rcall   send_cmd       ; send_command does separate the nibbles of
                               ; these numbers, 28, 0C, 06, and 80 (line1)
       ldi       time,10           ; wait 10ms
       rcall   delay

       ldi       r16,0x0c       ; Display on Cursor off
       rcall   send_cmd
       ldi       time,10           ; wait 10ms
       rcall   delay

       ldi       r16,0x06       ; Entry Mode (make LCD ready for data)
       rcall   send_cmd
       ldi       time,10           ; wait 10ms
       rcall   delay

       ldi       r16,line1       ; set cursor at line 1, column 1
       rcall   send_cmd
       ldi       time,10           ; wait 10ms
       rcall   delay

       ret

;=========================================================================
;***** Delay = 1ms. time holds the number of ms needed
;=========================================================================
delay:  
TDelay:   ldi delayReg,0       ; initialize timer/counter 0
       out TCNT0,delayReg  

TLoop:   in CntValue,TCNT0   ; read timer/counter value
       cpi CntValue,20       ; is it 20?
       brne TLoop               ; if not 20, read it again (until 0)
       dec time               ; counting down to 0 for # of ms
       brne TDelay
       ret

;=========================================================================
;***** Delay = calls delay 100 times (100ms). Longtime holds the # of 100
;                                            ms delays needed
;=========================================================================
Longdelay:  
       ldi       time,100
up:       rcall   delay
       dec    longtime       ; counting down to 0 for # of 100ms
       brne     up
       ret

;=========================================================================
; send_reset
; The 3pi interfaces the ATmega168 to the LCD in an unusual way. Instead of
; tying b0, b1, b2, and b3 (or 4, 5, 6, 7) of a port to b4, b5, b6, and b7
; of the LCD, the interface is connected as follows:
; LCD Bit4 <-> PortB, pin 1
; LCD Bit5 <-> PortB, pin 4
; LCD Bit6 <-> PortB, pin 6
; LCD Bit7 <-> PortD, pin 7
; this means that not only do the nibbles have to be separated because of the
; 4 bit interface, but you must them "build" the nibble in the ports to send
; out the command.
;
; In this routine R16 will hold the command
;=========================================================================
send_reset:
       clr       temp           ; temp will be used to hold the command
       sbrc   r16,3           ; see if bit 3 of the command is 0 in r16
       sbr       temp,0x80       ; if not, set bit 7 of port D
       out       PORTD,temp       ; send data out to PORTD

       clr       temp          
       sbrc   r16,0           ; see if bit 0 of the command is 0
       sbr       temp,0x02       ; if not, set bit 1 of r16
       sbrc   r16,1           ; see if bit 1 of the command is 0
       sbr       temp,0x10       ; if not, set b5 of r16
       sbrc   r16,2           ; see if bit 2 of the command is 0
       sbr       temp,0x20       ; if not. set b6 of r16
       out       PORTB,temp       ; send to the LCD
       sbi       PORTD,lcde       ; make enable go high
       nop                       ; short delay
       cbi       PORTD,lcde       ; make enable go low
       ldi       time,10           ; wait 10ms
       rcall   delay
       ret
;=========================================================================

;=========================================================================
; send_cmd.
; Because of the 4 bit interface, the 8 bit command must be separated into
; two nibble. The command must then be "built" as in the send_reset routine
; above.
;
; Note: the high nibble of the 8 bit command must be sent to the LCD 1st
;=========================================================================
send_cmd:
       mov       temp2,r16       ; save r16
       swap   r16               ; swap nibbles to put high nibble in the
                               ; lower 4 bits. the high nibble mus be sent 1st
       clr       temp           ; temp will be used to hold the command
       sbrc   r16,3           ; see if bit 3 of the command is 0 in r16
       sbr       temp,0x80       ; if not, set bit 7 of port D
       out       PORTD,temp       ; send data out to PORTD

       clr       temp          
       sbrc   r16,0           ; see if bit 0 of the command is 0
       sbr       temp,0x02       ; if not, set bit 1 of r16
       sbrc   r16,1           ; see if bit 1 of the command is 0
       sbr       temp,0x10       ; if not, set b5 of r16
       sbrc   r16,2           ; see if bit 2 of the command is 0
       sbr       temp,0x20       ; if not. set b6 of r16
       out       PORTB,temp       ; send to the LCD
       sbi       PORTD,lcde       ; make enable go high
       nop                       ; short delay
       cbi       PORTD,lcde       ; make enable go low

       ldi       time,10           ; wait 10ms
       rcall   delay

       swap   r16               ; get low nibble back to it's original position
       clr       temp           ; see if bit 0 of the command is 0
       sbrc   r16,3           ; see if bit 3 of the command is 0 in r16
       sbr       temp,0x80       ; if not, set bit 7 of port D
       out       PORTD,temp       ; send data out to PORTD

       clr       temp           ; see if bit 0 of the command is 0
       sbrc   r16,0           ; if not, set bit 1 of r16
       sbr       temp,0x02       ; see if bit 1 of the command is 0
       sbrc   r16,1           ; see if bit 1 of the command is 0
       sbr       temp,0x10       ; if not, set b5 of r16
       sbrc   r16,2           ; see if bit 2 of the command is 0
       sbr       temp,0x20       ; if not. set b6 of r16
       out       PORTB,temp       ; send to the LCD
       sbi       PORTD,lcde       ; make enable go high
       nop                       ; short delay
       cbi       PORTD,lcde       ; make enable go low
       ldi       time,10           ; wait 10ms
       rcall   delay

       ret
;=========================================================================

;=========================================================================
; send_char
; the send_char routine is almost identical to the one above - send_cmd.
; r16 holds the character to be sent. the only real difference is that RS
; must be set on the LCD for data entry
;=========================================================================
send_char:
       mov       temp2,r16       ; save r16
       swap   r16               ; swap nibbles to put high nibble in the
                               ; lower 4 bits. the high nibble mus be sent 1st
       clr       temp           ; temp will be used to hold the char
       sbrc   r16,3           ; see if bit 3 of the char is 0 in r16
       sbr       temp,0x80       ; if not, set bit 7 of port D
       sbr       temp,0x04       ;make sure RS is set
       out       PORTD,temp       ; send data out to PORTD

       clr       temp      
       sbrc   r16,0           ; see if bit 0 of the command is 0
       sbr       temp,0x02       ; if not, set bit 1 of r16
       sbrc   r16,1           ; see if bit 1 of the command is 0
       sbr       temp,0x10       ; if not, set b5 of r16
       sbrc   r16,2           ; see if bit 2 of the command is 0
       sbr       temp,0x20       ; if not. set b6 of r16
       out       PORTB,temp       ; send to the LCD
       sbi       PORTD,lcde       ; make enable go high
       nop                       ; short delay
       cbi       PORTD,lcde       ; make enable go low

       ldi       time,10           ; wait 10ms
       rcall   delay

       swap   r16
       clr       temp           ; temp will be used to hold the char
       sbrc   r16,3           ; see if bit 3 of the char is 0 in r16
       sbr       temp,0x80       ; if not, set bit 7 of port D
       sbr       temp,0x04       ;make sure RS is set
       out       PORTD,temp       ; send data out to PORTD

       clr       temp
       sbrc   r16,0           ; see if bit 0 of the command is 0
       sbr       temp,0x02       ; if not, set bit 1 of r16
       sbrc   r16,1           ; see if bit 1 of the command is 0
       sbr       temp,0x10       ; if not, set b5 of r16
       sbrc   r16,2           ; see if bit 2 of the command is 0
       sbr       temp,0x20       ; if not. set b6 of r16
       out       PORTB,temp       ; send to the LCD
       sbi       PORTD,lcde       ; make enable go high
       nop                       ; short delay
       cbi       PORTD,lcde       ; make enable go low

       ldi       time,10           ; wait 10ms
       rcall   delay

       ret
;=========================================================================

;=========================================================================
;       Clear entire LCD and delay for a bit
;=========================================================================
lcdclr:
       ldi       r16,0x01       ;Address increment, no scrolling
       rcall   send_cmd
       ldi       time,10           ; wait 10ms
       rcall   delay
       ret
;=========================================================================

;=========================================================================
;       Get Any Button Press
;=========================================================================
getbutton:
       ldi       r20,0b00001101   ;set some pins as inputs
       out       DDRB,r20
       ldi       r20,0b11110010   ;set pullups
       out       PORTB,r20   
again:   clr       r16
       ldi       time,10           ; wait 10ms
       rcall   delay
       in        r16,PINB
       sbrs   r16,1
       rjmp   ButtonA
       sbrs   r16,4
       rjmp   ButtonB
       sbrs   r16,5
       rjmp   ButtonC
       rjmp   again

ButtonA:                           ;forward then back
       ldi       r20,0b00111111        ;PB0,1,2,3,4,5 outputs, PB6 & 7 are Xtal
       out       DDRB,r20
       ldi       r20,0b11000000       ;enable internal pull-ups on PB6-PB7
       out       PORTB,r20
       ldi       longtime,5           ;# of 100ms
       rcall   Longdelay

       ldi       r20,5       ;slow   (changed)
       out       OCR0A,r20
       sts       OCR2A,r20
       ldi       r20,0
       out       OCR0B,r20
       sts       OCR2B,r20
       ldi       longtime,10           ;# of 100ms
       rcall   Longdelay

       ldi       r20,127       ;medium   (changed)
       out       OCR0A,r20
       sts       OCR2A,r20
       ldi       r20,0
       out       OCR0B,r20
       sts       OCR2B,r20
       ldi       longtime,10           ;# of 100ms
       rcall   Longdelay

       ldi       r20,255       ;full speed   (changed)
       out       OCR0A,r20
       sts       OCR2A,r20
       ldi       r20,0
       out       OCR0B,r20
       sts       OCR2B,r20
       ldi       longtime,10           ;# of 100ms
       rcall   Longdelay

       ret

ButtonB:
       ldi       r20,0b00111111        ;PB0,1,2,3,4,5 outputs, PB6 & 7 are Xtal
       out       DDRB,r20
       ldi       r20,0b11000000       ;enable internal pull-ups on PB6-PB7
       out       PORTB,r20   
       ldi       longtime,5           ;# of 100ms
       rcall   Longdelay
       cbi       PORTD,6            ;spin
       sbi       PORTD,5
       cbi       PORTD,3
       sbi       PORTB,3
       ldi       longtime,10           ;# of 100ms
       rcall   Longdelay
       sbi       PORTD,6            ;break
       sbi       PORTD,5
       sbi       PORTD,3
       sbi       PORTB,3
       ret

ButtonC:
       ldi       r20,0b00111111        ;PB0,1,2,3,4,5 outputs, PB6 & 7 are Xtal
       out       DDRB,r20
       ldi       r20,0b11000000       ;enable internal pull-ups on PB6-PB7
       out       PORTB,r20   
       ret

;=========================================================================

;=========================================================================
send_mess:
;=========================================================================
       lpm                   ; Load byte from program memory into r0

       tst       r0           ; Check if we've reached the end of the message
       breq   return       ; If so, quit

       mov       r16,r0
       rcall   send_char   ; Send received char

       adiw   ZL,1       ; Increment Z register
       rjmp   send_mess

return:   ret                   ;return
;=========================================================================

;=========================================================================
; Messages
;=========================================================================

message1:
.db   "Doc's 3",0x5c,0
message2:
.db   "A, B, C",0
message3:
.db   "Reset?",0
message4:
.db   "key: ",0

Explanation / Answer

const char welcome_line1[] PROGMEM = " Pololu";
const char welcome_line2[] PROGMEM = "3÷ Robot";
const char demo_name_line1[] PROGMEM = "Line";
const char demo_name_line2[] PROGMEM = "follower";

// A couple of simple tunes, stored in program space.
const char welcome[] PROGMEM = ">g32>>c32";
const char go[] PROGMEM = "L16 cdegreg4";

// Data for generating the characters used in load_custom_characters
// and display_readings. By reading levels[] starting at various
// offsets, we can generate all of the 7 extra characters needed for a
// bargraph. This is also stored in program space.
const char levels[] PROGMEM = {
0b00000,
0b00000,
0b00000,
0b00000,
0b00000,
0b00000,
0b00000,
0b11111,
0b11111,
0b11111,
0b11111,
0b11111,
0b11111,
0b11111
};

// This function loads custom characters into the LCD. Up to 8
// characters can be loaded; we use them for 7 levels of a bar graph.
void load_custom_characters()
{
lcd_load_custom_character(levels+0,0); // no offset, e.g. one bar
lcd_load_custom_character(levels+1,1); // two bars
lcd_load_custom_character(levels+2,2); // etc...
lcd_load_custom_character(levels+3,3);
lcd_load_custom_character(levels+4,4);
lcd_load_custom_character(levels+5,5);
lcd_load_custom_character(levels+6,6);
clear(); // the LCD must be cleared for the characters to take effect
}

// This function displays the sensor readings using a bar graph.
void display_readings(const unsigned int *calibrated_values)
{
unsigned char i;

for(i=0;i<5;i++) {
// Initialize the array of characters that we will use for the
// graph. Using the space, an extra copy of the one-bar
// character, and character 255 (a full black box), we get 10
// characters in the array.
const char display_characters[10] = {' ',0,0,1,2,3,4,5,6,255};

// The variable c will have values from 0 to 9, since
// calibrated values are in the range of 0 to 1000, and
// 1000/101 is 9 with integer math.
char c = display_characters[calibrated_values[i]/101];

// Display the bar graph character.
print_character(c);
}
}

// Initializes the 3pi, displays a welcome message, calibrates, and
// plays the initial music.
void initialize()
{
unsigned int counter; // used as a simple timer
unsigned int sensors[5]; // an array to hold sensor values

// This must be called at the beginning of 3pi code, to set up the
// sensors. We use a value of 2000 for the timeout, which
// corresponds to 2000*0.4 us = 0.8 ms on our 20 MHz processor.
pololu_3pi_init(2000);
load_custom_characters(); // load the custom characters

// Play welcome music and display a message
print_from_program_space(welcome_line1);
lcd_goto_xy(0,1);
print_from_program_space(welcome_line2);
play_from_program_space(welcome);
delay_ms(1000);

clear();
print_from_program_space(demo_name_line1);
lcd_goto_xy(0,1);
print_from_program_space(demo_name_line2);
delay_ms(1000);

// Display battery voltage and wait for button press
while(!button_is_pressed(BUTTON_B))
{
int bat = read_battery_millivolts();

clear();
print_long(bat);
print("mV");
lcd_goto_xy(0,1);
print("Press B");

delay_ms(100);
}

// Always wait for the button to be released so that 3pi doesn't
// start moving until your hand is away from it.
wait_for_button_release(BUTTON_B);
delay_ms(1000);

// Auto-calibration: turn right and left while calibrating the
// sensors.
for(counter=0;counter<80;counter++)
{
if(counter < 20 || counter >= 60)
set_motors(40,-40);
else
set_motors(-40,40);

// This function records a set of sensor readings and keeps
// track of the minimum and maximum values encountered. The
// IR_EMITTERS_ON argument means that the IR LEDs will be
// turned on during the reading, which is usually what you
// want.
calibrate_line_sensors(IR_EMITTERS_ON);

// Since our counter runs to 80, the total delay will be
// 80*20 = 1600 ms.
delay_ms(20);
}
set_motors(0,0);

// Display calibrated values as a bar graph.
while(!button_is_pressed(BUTTON_B))
{
// Read the sensor values and get the position measurement.
unsigned int position = read_line(sensors,IR_EMITTERS_ON);

// Display the position measurement, which will go from 0
// (when the leftmost sensor is over the line) to 4000 (when
// the rightmost sensor is over the line) on the 3pi, along
// with a bar graph of the sensor readings. This allows you
// to make sure the robot is ready to go.
clear();
print_long(position);
lcd_goto_xy(0,1);
display_readings(sensors);

delay_ms(100);
}
wait_for_button_release(BUTTON_B);

clear();

print("Go!");   

// Play music and wait for it to finish before we start driving.
play_from_program_space(go);
while(is_playing());
}

// This is the main function, where the code starts. All C programs
// must have a main() function defined somewhere.
int main()
{
unsigned int sensors[5]; // an array to hold sensor values

// set up the 3pi
initialize();

// This is the "main loop" - it will run forever.
while(1)
{
// Get the position of the line. Note that we *must* provide
// the "sensors" argument to read_line() here, even though we
// are not interested in the individual sensor readings.
unsigned int position = read_line(sensors,IR_EMITTERS_ON);

if(position < 1000)
{
// We are far to the right of the line: turn left.

// Set the right motor to 100 and the left motor to zero,
// to do a sharp turn to the left. Note that the maximum
// value of either motor speed is 255, so we are driving
// it at just about 40% of the max.
set_motors(0,100);

// Just for fun, indicate the direction we are turning on
// the LEDs.
left_led(1);
right_led(0);
}
else if(position < 3000)
{
// We are somewhat close to being centered on the line:
// drive straight.
set_motors(100,100);
left_led(1);
right_led(1);
}
else
{
// We are far to the left of the line: turn right.
set_motors(100,0);
left_led(0);
right_led(1);
}
}

// This part of the code is never reached. A robot should
// never reach the end of its program, or unpredictable behavior
// will result as random code starts getting executed. If you
// really want to stop all actions at some point, set your motors
// to 0,0 and run the following command to loop forever:
//
// while(1);
}

// Get the position of the line. Note that we *must* provide
// the "sensors" argument to read_line() here, even though we
// are not interested in the individual sensor readings.
unsigned int position = read_line(sensors,IR_EMITTERS_ON);

// The "proportional" term should be 0 when we are on the line.
int proportional = ((int)position) - 2000;

// Compute the derivative (change) and integral (sum) of the
// position.
int derivative = proportional - last_proportional;
integral += proportional;

// Remember the last position.
last_proportional = proportional;

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
// Compute the difference between the two motor power settings,
// m1 - m2. If this is a positive number the robot will turn
// to the right. If it is a negative number, the robot will
// turn to the left, and the magnitude of the number determines
// the sharpness of the turn.
int power_difference = proportional/20 + integral/10000 + derivative*3/2;

// Compute the actual motor settings. We never set either motor
// to a negative value.
const int max = 60;
if(power_difference > max)
power_difference = max;
if(power_difference < -max)
power_difference = -max;

if(power_difference < 0)
set_motors(max+power_difference, max);
else
set_motors(max, max-power_difference);

Related Questions

Navigate

• Browse (All)
• Browse P
• Subjects