I am trying to operate the 3pi as per the random 5 number digit generated. The numbers can be from 0-9 as their as 9 stations that the 3pi robot can move to. I get the part where i can generate random numbers, but am confused in how to write code for its movement. If i write code for every station to station , the program will be very long. Is their any shorter way of doing it?
Hi,
How your 3pi navigates around its world will dictate how this works. Are you using a grid, or a series of paths (much like the maze solver, except with multiple end points)?
If you’re using a series of paths (which I suspect you are, based on your previous posts), a table would work well, where each entry[i][j] refers to a series of moves (much like the path[] array in the maze solver) that will guide your 3pi from station i to station j.
Thanks,
Geoff
I have been trying to get the robot move as per the random generated number. The logic behind the code i wrote seems fine to me but it doesn’t operate properly. Can’t figure out what my mistake is. Here is the code:
The main file:
#include <pololu/3pi.h>
#include <avr/pgmspace.h>
#include <stdlib.h>
#include "turn.h"
#include "follow-segment.h"
const char welcome_line1[] PROGMEM = " Pololu";
const char welcome_line2[] PROGMEM = "3\xf7 Robot";
const char demo_name_line1[] PROGMEM = "Random";
const char demo_name_line2[] PROGMEM = "Maze";
const char welcome[] PROGMEM = ">g32>>c32";
int randNum[4];
int amount;
void initialize()
{
unsigned int counter;
unsigned int sensors[5];
pololu_3pi_init(2000);
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);
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);
}
wait_for_button_release(BUTTON_B);
delay_ms(1000);
for(counter=0;counter<80;counter++)
{
if(counter < 20 || counter >= 60)
set_motors(40,-40);
else
set_motors(-40,40);
calibrate_line_sensors(IR_EMITTERS_ON);
delay_ms(20);
}
set_motors(0,0);
}
void random_counter()
{
srand(get_ms() % 5000);
for(int z=0; z<4; ++z)
{
randNum[z] = rand()%10;
if(z != 0)
while(randNum[z] == randNum[z-1])
randNum[z] = rand()%10;
}
}
int main()
{
char n5to0[] = {'L','L','S','S','S','R'};
char n5to1[] = {'L','L','S','S','R'};
char n5to2[] = {'L','L','S','R'};
char n5to3[] = {'L','L','R'};
char n5to4[] = {'L','S'};
char n5to5[] = {'S'};
char n5to6[] = {'R','S'};
char n5to7[] = {'R','R','L'};
char n5to8[] = {'R','R','S','L'};
char n5to9[] = {'R','R','S','S','L'};
/////////////////////////
char p0to1[] = {'L','L'};
char p0to2[] = {'L','S','L'};
char p0to3[] = {'L','S','S','L'};
char p0to4[] = {'L','S','S','S','L'};
char p0to5[] = {'L','S','S','S','R','L'};
char p0to6[] = {'S','L','S','S','R'};
char p0to7[] = {'S','L','S','R'};
char p0to8[] = {'S','L','R'};
char p0to9[] = {'S','S'};
/////////////////////////
char p1to0[] = {'R','R'};
char p1to2[] = {'L','L'};
char p1to3[] = {'L','S','L'};
char p1to4[] = {'L','S','S','L'};
char p1to5[] = {'L','S','S','R','L'};
char p1to6[] = {'L','S','S','R','S','S'};
char p1to7[] = {'R','L','L','S','R'};
char p1to8[] = {'R','L','L','R'};
char p1to9[] = {'R','L','S'};
/////////////////////////
char p2to0[] = {'R','S','R'};
char p2to1[] = {'R','R'};
char p2to3[] = {'L','L'};
char p2to4[] = {'L','S','L'};
char p2to5[] = {'L','S','R','L'};
char p2to6[] = {'L','S','R','S','S'};
char p2to7[] = {'R','L','L','S','R'};
char p2to8[] = {'R','L','L','R'};
char p2to9[] = {'R','L','L','S'};
//////////////////////////
char p3to0[] = {'R','S','S','R'};
char p3to1[] = {'R','S','R'};
char p3to2[] = {'R','R'};
char p3to4[] = {'L','L'};
char p3to5[] = {'L','R','L'};
char p3to6[] = {'L','R','S','S'};
char p3to7[] = {'L','R','S','R','L'};
char p3to8[] = {'R','S','S','L','L','R'};
char p3to9[] = {'R','S','S','L','S'};
/////////////////////////
char p4to0[] = {'R','S','S','S','R'};
char p4to1[] = {'R','S','S','R'};
char p4to2[] = {'R','S','R'};
char p4to3[] = {'R','R'};
char p4to5[] = {'S','L'};
char p4to6[] = {'S','S','S'};
char p4to7[] = {'S','S','R','L'};
char p4to8[] = {'S','S','R','S','L'};
char p4to9[] = {'R','S','S','S','L','S'};
/////////////////////////
char p5to0[] = {'R','L','S','S','S','R'};
char p5to1[] = {'R','L','S','S','R'};
char p5to2[] = {'R','L','S','R'};
char p5to3[] = {'R','L','R'};
char p5to4[] = {'R','S'};
char p5to6[] = {'L','S'};
char p5to7[] = {'L','R','L'};
char p5to8[] = {'L','R','S','L'};
char p5to9[] = {'L','R','S','S','L'};
/////////////////////////
char p6to0[] = {'L','S','S','R','S'};
char p6to1[] = {'L','S','S','R','R','L'};
char p6to2[] = {'L','S','S','R','R','S','L'};
char p6to3[] = {'L','S','S','R','R','S','S','L'};
char p6to4[] = {'S','S','S'};
char p6to5[] = {'S','R'};
char p6to7[] = {'L','L'};
char p6to8[] = {'L','S','L'};
char p6to9[] = {'L','S','S','L'};
/////////////////////////
char p7to0[] = {'L','S','R','S'};
char p7to1[] = {'L','S','R','R','L'};
char p7to2[] = {'L','S','R','R','S','L'};
char p7to3[] = {'R','L','S','L','R'};
char p7to4[] = {'R','L','S','S'};
char p7to5[] = {'R','L','R'};
char p7to6[] = {'R','R'};
char p7to8[] = {'L','L'};
char p7to9[] = {'L','S','L'};
/////////////////////////
char p8to0[] = {'L','R','S'};
char p8to1[] = {'L','R','R','L'};
char p8to2[] = {'L','R','R','S','L'};
char p8to3[] = {'L','R','R','S','S','L'};
char p8to4[] = {'R','S','L','S','S'};
char p8to5[] = {'R','S','L','R'};
char p8to6[] = {'R','S','R'};
char p8to7[] = {'R','R'};
char p8to9[] = {'L','L'};
///////////////////////////
char p9to0[] = {'S','S'};
char p9to1[] = {'S','R','L'};
char p9to2[] = {'S','R','S','L'};
char p9to3[] = {'S','R','S','S','L'};
char p9to4[] = {'S','R','S','S','S','L'};
char p9to5[] = {'R','S','S','L','R'};
char p9to6[] = {'R','S','S','R'};
char p9to7[] = {'R','S','R'};
char p9to8[] = {'R','R'};
char pathway[10];
int CP=-5;
int Destination;
initialize();
random_counter();
follow_segment();
set_motors(70,70);
delay_ms(30);
while(1)
{
for(int i=0; i<4; ++i)
{
Destination = randNum[i];
switch(CP)
{
case -5:
switch(Destination)
{
case 0:
for(int i = 0; i < 6; ++i)
{
pathway[i] = n5to0[i];
}
amount=6;
break;
case 1:
for(int i = 0; i < 5; ++i)
{
pathway[i] = n5to1[i];
}
amount=5;
break;
case 2:
for(int i = 0; i < 4; ++i)
{
pathway[i] = n5to2[i];
}
amount=4;
break;
case 3:
for(int i = 0; i < 3; ++i)
{
pathway[i] = n5to3[i];
}
amount=3;
break;
case 4:
for(int i = 0; i < 2; ++i)
{
pathway[i] = n5to4[i];
}
amount=2;
break;
case 5:
for(int i = 0; i < 1; ++i)
{
pathway[i] = n5to5[i];
}
amount=1;
break;
case 6:
for(int i = 0; i < 2; ++i)
{
pathway[i] = n5to6[i];
}
amount=2;
break;
case 7:
for(int i = 0; i < 3; ++i)
{
pathway[i] = n5to7[i];
}
amount=3;
break;
case 8:
for(int i = 0; i < 4; ++i)
{
pathway[i] = n5to8[i];
}
amount=4;
break;
case 9:
for(int i = 0; i < 5; ++i)
{
pathway[i] = n5to9[i];
}
amount=5;
break;
}
CP = Destination;
break;
case 0:
switch(Destination)
{
case 1:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p0to1[i];
}
amount=2;
break;
case 2:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p0to2[i];
}
amount=3;
break;
case 3:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p0to3[i];
}
amount=4;
break;
case 4:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p0to4[i];
}
amount=5;
break;
case 5:
for(int i = 0; i < 6; ++i)
{
pathway[i] = p0to5[i];
}
amount=6;
break;
case 6:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p0to6[i];
}
amount=5;
break;
case 7:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p0to7[i];
}
amount=4;
break;
case 8:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p0to8[i];
}
amount=3;
break;
case 9:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p0to9[i];
}
amount=2;
break;
}
CP = Destination;
break;
case 1:
switch(Destination)
{
case 0:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p1to0[i];
}
amount=2;
break;
case 2:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p1to2[i];
}
amount=2;
break;
case 3:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p1to3[i];
}
amount=3;
break;
case 4:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p1to4[i];
}
amount=4;
break;
case 5:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p1to5[i];
}
amount=5;
break;
case 6:
for(int i = 0; i < 6; ++i)
{
pathway[i] = p1to6[i];
}
amount=6;
break;
case 7:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p1to7[i];
}
amount=5;
break;
case 8:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p1to8[i];
}
amount=4;
break;
case 9:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p1to9[i];
}
amount=3;
break;
}
CP = Destination;
break;
case 2:
switch(Destination)
{
case 0:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p2to0[i];
}
amount=3;
break;
case 1:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p2to1[i];
}
amount=2;
break;
case 3:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p2to3[i];
}
amount=2;
break;
case 4:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p2to4[i];
}
amount=3;
break;
case 5:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p2to5[i];
}
amount=4;
break;
case 6:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p2to6[i];
}
amount=5;
break;
case 7:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p2to7[i];
}
amount=5;
break;
case 8:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p2to8[i];
}
amount=4;
break;
case 9:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p2to9[i];
}
amount=4;
break;
}
CP = Destination;
break;
case 3:
switch(Destination)
{
case 0:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p3to0[i];
}
amount=6;
break;
case 1:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p3to1[i];
}
amount=3;
break;
case 2:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p3to2[i];
}
amount=2;
break;
case 4:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p3to4[i];
}
amount=2;
break;
case 5:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p3to5[i];
}
amount=3;
break;
case 6:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p3to6[i];
}
amount=4;
break;
case 7:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p3to7[i];
}
amount=5;
break;
case 8:
for(int i = 0; i < 6; ++i)
{
pathway[i] = p3to8[i];
}
amount=6;
break;
case 9:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p3to9[i];
}
amount=5;
break;
}
CP = Destination;
break;
case 4:
switch(Destination)
{ case 0:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p4to0[i];
}
amount=5;
break;
case 1:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p4to1[i];
}
amount=4;
break;
case 2:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p4to2[i];
}
amount=3;
break;
case 3:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p4to3[i];
}
amount=2;
break;
case 5:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p4to5[i];
}
amount=2;
break;
case 6:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p4to6[i];
}
amount=3;
break;
case 7:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p4to7[i];
}
amount=4;
break;
case 8:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p4to8[i];
}
amount=5;
break;
case 9:
for(int i = 0; i < 6; ++i)
{
pathway[i] = p4to9[i];
}
amount=6;
break;
}
CP = Destination;
break;
case 5:
switch(Destination)
{
case 0:
for(int i = 0; i < 6; ++i)
{
pathway[i] = p5to0[i];
}
amount=6;
break;
case 1:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p5to1[i];
}
amount=5;
break;
case 2:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p5to2[i];
}
amount=4;
break;
case 3:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p5to3[i];
}
amount=3;
break;
case 4:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p5to4[i];
}
amount=2;
break;
case 6:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p5to6[i];
}
amount=2;
break;
case 7:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p5to7[i];
}
amount=3;
break;
case 8:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p5to8[i];
}
amount=4;
break;
case 9:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p5to9[i];
}
amount=5;
break;
}
CP = Destination;
break;
case 6:
switch(Destination)
{
case 0:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p6to0[i];
}
amount=5;
break;
case 1:
for(int i = 0; i < 6; ++i)
{
pathway[i] = p6to1[i];
}
amount=6;
break;
case 2:
for(int i = 0; i < 7; ++i)
{
pathway[i] = p6to2[i];
}
amount=7;
break;
case 3:
for(int i = 0; i < 8; ++i)
{
pathway[i] = p6to3[i];
}
amount=8;
break;
case 4:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p6to4[i];
}
amount=3;
break;
case 5:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p6to5[i];
}
amount=2;
break;
case 7:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p6to7[i];
}
amount=2;
break;
case 8:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p6to8[i];
}
amount=3;
break;
case 9:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p6to9[i];
}
amount=4;
break;
}
CP = Destination;
break;
case 7:
switch(Destination)
{
case 0:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p7to0[i];
}
amount=4;
break;
case 1:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p7to1[i];
}
amount=5;
break;
case 2:
for(int i = 0; i < 6; ++i)
{
pathway[i] = p7to2[i];
}
amount=6;
break;
case 3:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p7to3[i];
}
amount=5;
break;
case 4:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p7to4[i];
}
amount=4;
break;
case 5:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p7to5[i];
}
amount=3;
break;
case 6:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p7to6[i];
}
amount=2;
break;
case 8:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p7to8[i];
}
amount=2;
break;
case 9:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p7to9[i];
}
amount=3;
break;
}
CP = Destination;
break;
case 8:
switch(Destination)
{
case 0:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p8to0[i];
}
amount=3;
break;
case 1:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p8to1[i];
}
amount=4;
break;
case 2:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p8to2[i];
}
amount=5;
break;
case 3:
for(int i = 0; i < 6; ++i)
{
pathway[i] = p8to3[i];
}
amount=6;
break;
case 4:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p8to4[i];
}
amount=5;
break;
case 5:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p8to5[i];
}
amount=4;
break;
case 6:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p8to6[i];
}
amount=3;
break;
case 7:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p8to7[i];
}
amount=2;
break;
case 9:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p8to9[i];
}
amount=2;
break;
}
CP = Destination;
break;
case 9:
switch(Destination)
{
case 0:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p9to0[i];
}
amount=2;
break;
case 1:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p9to1[i];
}
amount=3;
break;
case 2:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p9to2[i];
}
amount=4;
break;
case 3:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p9to3[i];
}
amount=5;
break;
case 4:
for(int i = 0; i < 6; ++i)
{
pathway[i] = p9to4[i];
}
amount=6;
break;
case 5:
for(int i = 0; i < 5; ++i)
{
pathway[i] = p9to5[i];
}
amount=5;
break;
case 6:
for(int i = 0; i < 4; ++i)
{
pathway[i] = p9to6[i];
}
amount=4;
break;
case 7:
for(int i = 0; i < 3; ++i)
{
pathway[i] = p9to7[i];
}
amount=3;
break;
case 8:
for(int i = 0; i < 2; ++i)
{
pathway[i] = p9to8[i];
}
amount=2;
break;
}
CP = Destination;
break;
}
}
for(int j=0;j < amount;++j)
{
follow_segment();
set_motors(70,70);
delay_ms(30);
set_motors(40,40);
delay_ms(200);
turn(pathway[j]);
}
follow_segment();
turn('B');
}
} The follow_segment:
#include <pololu/3pi.h>
void follow_segment()
{
int last_proportional = 0;
long integral=0;
while(1)
{
unsigned int sensors[5];
unsigned int position = read_line(sensors,IR_EMITTERS_ON);
int proportional = ((int)position) - 2000;
int derivative = proportional - last_proportional;
integral += proportional;
last_proportional = proportional;
int power_difference = proportional/20 + integral/10000 + derivative*3/2;
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);
if(sensors[1] < 100 && sensors[2] < 100 && sensors[3] < 100)
{
return;
}
else if(sensors[0] > 600 || sensors[4] > 600)
{
return;
}
}
}The turn function:
#include <pololu/3pi.h>
void turn(char dir)
{
switch(dir)
{
case 'L':
set_motors(-80,80);
delay_ms(200);
break;
case 'R':
set_motors(80,-80);
delay_ms(200);
break;
case 'B':
set_motors(80,-80);
delay_ms(400);
break;
case 'S':
break;
}
}Hello.
How do you expect it to operate and how does it operate?
It looks like your code could probably be simplified greatly with the use of multidimensional arrays.
Can you write the simplest program that does not do what you expect and tell us what you want it to do and what it does do?
- Ryan