int main()
{
_MODE = PRACTICE_MODE;
_ROBOT = LEGO;
initialize_camera(LOW_RES);
//point2 xy = get_camera_reading();
//DESIRED_X = xy.x;
//DESIRED_Y = xy.y;
set_servo_position(LIFT_SERVO, LIFT_SERVO_UP_POSITION);
set_servo_position(DUMP_SERVO, DUMP_UP_POSITION);
enable_servos();
//wait_for_light(3);
//shut_down_in(119);
pd_follow(STOPPING_TOPHAT, 0);
lego_spin_degrees(90, 50, RIGHT);
lego_drive_distance(16, 50, FORWARDS);
//suck_up_pom();
lego_spin_degrees(50, 50, RIGHT);
//suck_up_pom();
lego_spin(70, LEFT);
while (analog10(LREFLECTANCE) < 400);
while (analog10(RREFLECTANCE) < 400);
lego_stop();
line_follow(STOPPING_TIME, 3);
lego_drive_distance(2, 100, FORWARDS);
lego_drive_distance(2, 100, BACKWARDS);
lego_spin_degrees(40, 90, LEFT);
suck_up_pom();
lego_spin_degrees(80, 90, RIGHT);
suck_up_pom();
lego_spin(40, RIGHT);
sleep(2);
while (analog10(RREFLECTANCE) < 100);
while (analog10(LREFLECTANCE) < 100);
lego_stop();
line_follow(STOPPING_TOPHAT, 0);
lego_spin_degrees(90, 50, LEFT);
lego_spin(70, RIGHT);
while (analog10(RREFLECTANCE) < 400);
while (analog10(LREFLECTANCE) < 400);
lego_stop();
sleep(20);
line_follow(STOPPING_TIME, 3);
lego_drive_distance(7, 70,FORWARDS);
lego_spin_degrees(100, 50, RIGHT);
lego_drive_distance(10, 100, BACKWARDS);
set_servo_position(DUMP_SERVO, DUMP_DOWN_POSITION);
sleep(1);
set_servo_position(DUMP_SERVO, DUMP_UP_POSITION);
set_servo_position(DUMP_SERVO, DUMP_DOWN_POSITION);
return 0;
}
void jfolo(unsigned char count){
unsigned char junction_count=0;
unsigned char timer=0;
Forward(); // set motor move forward
// sending display to lcd takes long time
// junction reading and display less frequent.
// use timer variable to count 20, then read once.
while(junction_count<count){ //second junction
timer++;
if(timer>50){
timer=0; //clear timer
motor(0,0); //slow down for lcd display
do{
junction_count=LSA08_GetJunction(); //check junction count
}while(ERR_FLAG||(junction_count>10)); //checking no uart error
lcd_goto(0);
lcd_putchar('J');
lcd_num(junction_count,3);
}
line_follow(); //PID line follow
}
Brake();
__delay_ms(200);
}
/* 直線まで移動し止まる */
int stop_line( OdometryPtr odm, SpurUserParamsPtr spur )
{
double a;
double q;
double vel;
int over;
double x, y;
double nv, nw;
double dt;
reference_speed( &nv, &nw );
dt = p( YP_PARAM_CONTROL_CYCLE, 0 ) * 1.5;
x = odm->x + nv * cos( odm->theta ) * dt;
y = odm->y + nv * sin( odm->theta ) * dt;
a = ( x - spur->x ) * cos( spur->theta ) + ( y - spur->y ) * sin( spur->theta );
vel = timeoptimal_servo(
a,
spur->v,
0,
spur->dv );
over = 0;
if( fabs( spur->v ) <= fabs( vel ) )
{
SpurUserParams spur_line;
spur_line = *spur;
spur_line.v = -SIGN( a ) * fabs( spur->v );
line_follow( odm, &spur_line );
}
else
{
q = odm->theta - spur->theta;
q = trans_q( q );
regurator( 0, q, 1000, vel, spur->w, spur );
}
if( a > 0.05 )
{
// 越えている
over = 3;
}
else if( a < -0.05 )
{
// まだ
over = 1;
}
else
{
// 大体乗った
over = 2;
}
return over;
}
int main()
{
create_connect();
while (side_button() == 0)
{
line_follow();
msleep(10);
}
create_disconnect();
return 0;
}
void main (void){
initIO();
pwm_init();
motor(0,0);
lcd_init();
LSA08_Init();
__delay_ms(100);
lcd_clr();
lcd_goto(0);
lcd_putstr("LSA08");
lcd_goto(20);
lcd_putstr("Demo");
while(SW1);
while(SW1==0);
lcd_clr();
PID_MENU();
lcd_clr();
lcd_goto(0);
lcd_putstr("line\nfollow");
if (junction_mode){
junction_follow();
}
else {
while(1){
line_follow();
}
}
}
//main stuff haha
int main()
{
int claw = 1; //claw port
int arm = 0; //arm port
create_connect();
printf("create connected");
set_create_total_angle(0);
//create setup
turn_for(10, -200); //Turns the create around
//does some important stuff
create_drive_direct(-100, -100);
set_servo_position(claw, 1561);
enable_servos();
msleep(300);
set_servo_position(arm, 1850); //arm down
msleep(500);
set_servo_position(claw, 1000);
msleep(500);
set_servo_position(arm, 1643); //grabs ball
msleep(500);
turn_for(36, 200); //spins onto first black path to line follow later
msleep(500);
forward_for(50, -100); //moves forward onto the black line
line_follow(blackThresh , 212); //starts following the black line
set_servo_position(claw, 1561); //drops ball
//dropped ball already
msleep(400);
forward_for(500, 100); //back it up back it up
set_servo_position(arm, 245); //holds the arm up
msleep(100);
set_create_distance(0);
set_create_total_angle(0);
while (get_create_total_angle() > -10) { //spins itself onto the angle of other black line
create_spin_CW(200);
}
create_spin_CW(0); //stops spinning
msleep(1500);
create_drive_direct(-200, -200); //goes onto the black line to line follow later
msleep(1000);
create_drive_direct(0,0); //stops itself
msleep(500);
set_create_total_angle(0);
while (get_create_total_angle() > -3) { //angles itself a little bit
create_spin_CW(200);
}
create_spin_CW(0); //stops spinning
msleep(400);
create_drive_direct(-200, -200); //Drives forward a little bit to the next black line
msleep(1500);
create_drive_direct(0,0);
set_create_total_angle(0);
while (get_create_total_angle() > -7) { //Angles itself so it can drive in the middle
create_spin_CW(200);
}
create_spin_CW(0); //stops spinning
create_drive_direct(-230,-230); //Drives itself into the middle lane
msleep(4000);
create_drive_direct(0,0); //Reset stuff
set_create_total_angle(0);
while(analog(0) < blackThresh) { //Align itself using line following in the middle lane
create_spin_CW(50);
printf("Value is %d \n", analog(0));
if(digital(0) >= blackThresh) {
printf("I have found the middle black line!");
create_spin_CW(1); //Angle itself so it is exactly on the black line
}
}
create_spin_CW(0); //resets something for some reason why is this here?
set_servo_position(claw, 1561); //grabs the red cube
msleep(200);
set_servo_position(arm, 1984);
msleep(500);
set_servo_position(claw, 1144);
msleep(500);
return 0;
}
int main()
{
line_follow();
}
