void cross_turn_right()
{
unsigned int left,right;
while(1)
{
left = analog(0);
right = analog(2);
if(left>REF && right>REF)
{
forward(10);
}
else if(left<REF && right>REF)
{
turn_left(10);
}
else if(left>REF && right<REF)
{
turn_right(10);
}
else if(left<REF && right<REF)
{
forward(300);
turn_right(800);
break;
}
}
}
void MyRobot::run()
{
while (step(_time_step) != -1)
{
//Get the encoder values
_left_encoder = getLeftEncoder();
_right_encoder = getRightEncoder();
//Converts rad to meters
if(flag == 2 || flag == 8){
_distance = _right_encoder/ENCODER_RESOLUTION*WHEEL_RADIO;
}else{
_distance = _left_encoder/ENCODER_RESOLUTION*WHEEL_RADIO;
}
print_odometry_data();
//switch case to make the necesary steps to avoid the obstacle
switch (flag){
case 1:
cout << "linea recta"<< endl;
goStraight(5);
break;
case 2:
turn_left();
break;
case 3:
goStraight(3.5);
break;
case 4:
turn_right();
break;
case 5:
goStraight( 7);
break;
case 6:
turn_right();
break;
case 7:
goStraight(3.5);
break;
case 8:
turn_left();
break;
case 9:
goStraight(5);
break;
default:
_left_speed = 0;
_right_speed = 0;
break;
}
setSpeed(_left_speed, _right_speed);
}
}
void turn_around()
{
turn_right();
turn_right();
}
void main()
{
unsigned int dist,dist_l,dist_r;
servo(0,GET_MID);
lcd("Press SW1");
sw1_press();
while(1)
{
servo(0,GET_MID);
dist = srf05_dist(ECHO_PB4,PULSE_PB1);
if(dist>=3 && dist<=12)
{
pause();
servo(0,GET_LEFT);
sleep(2000);
dist_l = srf05_dist(ECHO_PB4,PULSE_PB1);
servo(0,GET_RIGHT);
sleep(2000);
dist_r = srf05_dist(ECHO_PB4,PULSE_PB1);
servo(0,GET_MID);
if(dist_l>dist_r && dist_l>DIST_MIN)
{
turn_left(200);
}
else if(dist_r>dist_l && dist_r>DIST_MIN)
{
turn_right(200);
}
else if(dist_r==dist_l && dist_r>DIST_MIN)
{
turn_right(200);
}
else
{
backward(1000);
turn_right(500);
}
}
else
{
forward(10);
}
}
}
static void insert_balance(struct node_st **tree, struct node_st *node)
{
while (node != *tree && node->p->color == RED) {
if (node->p == node->p->p->l) {
/* case 1 */
if (node->p->p->r->color == RED) {
node->p->color = node->p->p->r->color = BLACK;
node->p->p->color = RED;
node = node->p->p;
continue;
}
/* case 2 */
if (node == node->p->r) {
node = node->p;
turn_left(node, tree);
}
/* case 3 */
node->p->color = BLACK;
node->p->p->color = RED;
turn_right(node->p->p, tree);
} else {
/* case 1 */
if (node->p->p->l->color == RED) {
node->p->color = node->p->p->l->color = BLACK;
node->p->p->color = RED;
node = node->p->p;
continue;
}
/* case 2 */
if (node == node->p->l) {
node = node->p;
turn_right(node, tree);
}
/* case 3 */
node->p->color = BLACK;
node->p->p->color = RED;
turn_left(node->p->p, tree);
}
node = *tree;
}
(*tree)->color = BLACK;
}
int main() {
all_init();
while(1) {
move_forward(SIDE_LENGTH);
turn_right();
}
}
void main()
{
unsigned int left,mid,right;
lcd("Press SW1");
sw1_press();
while(1)
{
left = analog(0);
mid = analog(1);
right = analog(2);
if(left<REF && mid>REF && right<REF)
{
forward(10);
}
else if(left>REF && mid<REF && right<REF)
{
turn_left(10);
}
else if(left<REF && mid<REF && right>REF)
{
turn_right(10);
}
else if(left>REF && mid>REF && right>REF)
{
pause();
sleep(3000);
forward(500);
}
}
}
void main()
{
char left,right;
lcd("Press SW1");
sw1_press();
while(1)
{
left = in_d(0);
right = in_d(1);
if(left==1 && right==1)
{
forward(10);
}
else if(left==0 && right==1)
{
backward(1000);
turn_right(800);
}
else if(left==1 && right==0)
{
backward(1000);
turn_left(800);
}
else if(left==0 && right==0)
{
backward(1000);
turn_left(1500);
}
}
}
void Legs::move_according_state(){
/*
Given the current state, update the moviment.
*/
switch (get_current_state()) {
case FORWARD:
move_forward();
break;
case BACKWARD:
move_backward();
break;
case LEFT:
turn_left();
break;
case RIGHT:
turn_right();
break;
case BYELEFT:
bye_bye_left();
break;
case BYERIGHT:
bye_bye_right();
break;
case STOP:
zero_pos();
break;
default:
zero_pos();
break;
}
}
int main(void) {
const unsigned short MainCycle = 60;
Init(MainCycle);
for (;;) {
step();
if (is_black() == 0){
LED(3);
}else if((is_black_left() == 1) && (is_black_right() == 0)){
turn_left();
LED(2);
}else if((is_black_left() == 0) && (is_black_right() == 1)){
turn_right();
LED(1);
}else if((is_black_left() == 1) && (is_black_right() == 1)){
stop();
LED(0);
break;
}
}
stop();
return 0;
}
int launcher(t_box *box)
{
char buf[1];
int dir;
jump;
ft_putstr("\033[35mWHICH DIRECTION FOR THE NEXT MOVE ? :\033[0m");
jump;
while (read(0, buf, sizeof(buf)))
{
if (!(dir = check(buf)))
continue ;
if (dir == 2)
turn_right(box);
if (dir == 1)
turn_left(box);
if (dir == 3)
turn_up(box);
if (dir == 4)
turn_down(box);
ft_putstr("YOUR SCORE IS : ");
ft_putnbr(box->score);
jump;
jump;
ft_putstr("\033[35mAND NOW ? : \033[0m");
jump;
init(box, (ft_random((box->size * box->size), 0)));
print_tab(box);
}
return (0);
}
void slalom(void)
{
int x,y;
while (1) {
x = ADRead(0);
y = ADRead(1);
if (x>THRES&&y>THRES) { ///黒黒
stop();Wait(250); break;
}
if (x>THRES&&y<THRES) { //黒白
turn_left();Wait(50);
}
if (x<THRES&&y>THRES) { //白黒
turn_right();Wait(50);
}
if (x<THRES&&y<THRES) { //白白
forward();
}
}
}
void templateAppToucheBegan( float x, float y, unsigned int tap_count, unsigned int id )
{
openBrowser=false;
if( game_state == 1 && tap_count >= 2 ) game_state = 2;
if( x < ( screen_width * 0.5f ) )
{
if(y>screen_height*0.2)
{
if(!turn_left()&&!game_state)
{
touch_type[id]=1;
console_print("move_change");
}
}
}
else
{
if(y>screen_height*0.2)
{
if(!turn_right()&&!game_state)
{
touch_type[id]=2;
console_print("view_change");
}
}
}
}
void back()
{
turn_robot_left();
while (is_space_in_front_of_robot_clear())
{
move_robot_forwards();
}
turn_right();
}
int main()
{
go_straight();
turn_right();
go_upstair();
turn_left();
enter_restroom();
return 0;
}
void autonomouswalk_walk(uint8_t sensors[5])
{
uint8_t leftSideAlgorithm = navigation_left_algorithm();
if(leftSideAlgorithm)
{
if(navigation_check_left_turn(sensors[0], sensors[2]) == 2)
{
turn_left();
}
else if(sensors[4] > CORRIDOR_WIDTH)
{
walk_forward(sensors);
}
else if(navigation_check_right_turn(sensors[1], sensors[3]) == 2)
{
turn_right();
}
else
{
turn_around(sensors[4]);
}
}
else
{
if(navigation_check_right_turn(sensors[1], sensors[3]) == 2)
{
turn_left();
}
else if(sensors[4] > CORRIDOR_WIDTH)
{
walk_forward(sensors);
}
else if(navigation_check_left_turn(sensors[0], sensors[2]) == 2)
{
turn_right();
}
else
{
turn_around(sensors[4]);
}
}
}
void run_instruction(int x, int y, char ort, int row, int col)
{
int i, tx, ty;
enum { NIL, T } lost = NIL;
for(i = 0; instruction[i]; i++)
{
if(instruction[i] == 'L')
ort = turn_left(ort);
if(instruction[i] == 'R')
ort = turn_right(ort);
if(instruction[i] == 'F')
{
tx = x, ty = y;
/* move forward */
if(ort == 'N') y -= 1;
else if(ort == 'E') x += 1;
else if(ort == 'S') y += 1;
else if(ort == 'W') x -= 1;
/* out of grid, lost */
if(x < 0 || x > col)
{
x = tx;
if(grid[x][y] == 0)
{
grid[x][y] = 1;
lost = T;
break;
}
}
if(y < 0 || y > row)
{
y = ty;
if(grid[x][y] == 0)
{
grid[x][y] = 1;
lost = T;
break;
}
}
}
}
if(lost)
printf("%d %d %c LOST\n", x, row - y, ort);
else
printf("%d %d %c\n", x, row - y, ort);
}
/*Line follow: arguments are whatever threshold we are using,
plus however many iterations we want the function to perform*/
void line_follow(int threshold, int distance) {
while(get_create_distance() < distance) {
printf("Value: %d \n" , get_create_distance());
msleep(100);
if(analog(0)>=threshold) {
turn_right(3);
}
if(analog(0)<=threshold) {
turn_left(3);
}
}
}
void go_straight(void)
{
// very simple motor control
if ( SeeLine.b.Center ) straight_fwd();
else if (SeeLine.b.Left) spin_left();
else if (SeeLine.b.CntLeft) turn_left();
else if (SeeLine.b.CntRight) turn_right();
else if (SeeLine.b.Right) spin_right();
if ( (SeeLine.B ) == 0b00000u) motors_brake_all();
}
int main()
{
int i;
setup_movement();
ahead();
turn_right();
turn_left();
}
//Main function
int main(void)
{
//Initialize value array with 0
int i;
for(i=0;i<1000;i++){
val_array[i] = 0;
}
data_pos = 0;
bot_pos = 0;
init_devices();
//LCD_Reset_4bit();
lcd_cursor(2,1);
lcd_string("DRAWOID");
while(1){
if (bot_pos < data_pos){
lcd_print(1,9,val_array[bot_pos],3);
lcd_cursor(1,1);
if (mov_array[bot_pos] == 'F'){
lcd_string("F");
move_straight(val_array[bot_pos],1);
}
else if (mov_array[bot_pos] == 'B'){
lcd_string("B");
move_straight(val_array[bot_pos],0);
}
else if (mov_array[bot_pos] == 'R'){
lcd_string("R");
turn_right(val_array[bot_pos]);
}
else if (mov_array[bot_pos] == 'L'){
lcd_string("L");
turn_left(val_array[bot_pos]);
}
else if (mov_array[bot_pos] == 'U'){
lcd_string("U");
pen_up();
}
else if (mov_array[bot_pos] == 'D'){
lcd_string("D");
pen_down();
}
bot_pos++;
_delay_ms(1000);
}
}
return 0;
}
void move_wall(t_game *p)
{
if (p->algo == 1)
front(p);
else if (p->algo == 2)
cone(p);
else if (p->algo == 3)
turn_left(p);
else if (p->algo == 4)
turn_right(p);
else if (p->algo == 5)
reverse_cone(p);
}
void perform_action(gamestate_t *gs, int action)
{
switch(action)
{
case 0: noop(gs); qbLog("Noop"); break;
case 1: turn_left(gs); qbLog("turn left"); break;
case 2: turn_right(gs); qbLog("turn right"); break;
case 3: move_forward(gs); qbLog("move forward"); break;
case 4: move_backward(gs); qbLog("move backward"); break;
case 5: jump(gs); qbLog("jump"); break;
case 6: shoot(gs); qbLog("shoot"); break;
}
}
int loop_event(t_app *app)
{
if (app->player.move_forward == 1)
move_forward(app);
if (app->player.move_backward == 1)
move_backward(app);
if (app->player.turn_left == 1)
turn_left(app);
if (app->player.turn_right == 1)
turn_right(app);
refresh(app);
return (0);
}
void main()
{
setup_timer_2(T2_DIV_BY_16,255,1); //4.0 ms overflow, 4.0 ms interrupt
setup_ccp1(CCP_PWM);
setup_ccp2(CCP_PWM);
set_pwm1_duty((int16)200);
set_pwm2_duty((int16)200);
enable_interrupts(INT_SSP);
enable_interrupts(GLOBAL);
SET_TRIS_B(0);
stop();
//delay_ms(500);
while(TRUE)
{
if(hasCommand)
{
hasCommand= FALSE;
if(buffer[0] == 's'){
show();
}
else
switch(buffer[1])
{
case 1:
go_forward();
break;
case 2:
go_backward();
break;
case 3:
turn_right();
break;
case 4:
turn_left();
break;
case 5:
stop();
break;
default :
break;
}
}
}
}
void loop()
{
if (Serial.available() > 0)
{
int command = Serial.read();
Serial.print(command, DEC);
Serial.print(" ");
switch (command)
{
case 1:
move_stop();
delay(500);
move_forward();
break;
case 2:
move_stop();
delay(500);
turn_left();
break;
case 3:
move_stop();
delay(500);
turn_right();
break;
case 4:
move_stop();
delay(500);
move_backward();
break;
case 5:
move_stop();
break;
case 6:
front_led_control(true);
break;
case 7:
front_led_control(false);
break;
case 8:
rear_led_control(true);
break;
case 9:
rear_led_control(false);
break;
default:
move_stop();
front_led_control(false);
rear_led_control(false);
}
}
}
void put_poms_out(){
printf("put poms out \n");
turn_left();
double sec= seconds();
while(analog10(LEFT_SENSOR)<left_blk-150&&analog10(RIGHT_SENSOR)<right_blk-150&&(sec+1)>seconds()){ }
forward();
while(analog10(LEFT_SENSOR)<left_blk-150&&analog10(RIGHT_SENSOR)<right_blk-150){}
if(analog10(LEFT_SENSOR)>=left_blk-150&&analog10(RIGHT_SENSOR)>=right_blk-150){
put_out();
}
else if(analog10(LEFT_SENSOR)>=left_blk-150){
turn_left();
while(analog10(RIGHT_SENSOR)>=right_blk-150){}
stop();
msleep(1000);
forward();
while(analog10(LEFT_SENSOR)<left_blk-150){}
stop();
msleep(1000);
turn_left();
while(analog10(RIGHT_SENSOR)<right_blk-150){}
put_out();
}
else if(analog10(RIGHT_SENSOR)>=right_blk-150){
turn_right();
while(analog10(LEFT_SENSOR)>=left_blk-150){}
stop();
msleep(1000);
forward();
while(analog10(RIGHT_SENSOR)<right_blk-150){}
stop();
msleep(1000);
turn_right();
while(analog10(LEFT_SENSOR)<left_blk-150){}
put_out();
}
}
void segue_coordenadas(int8 dados)
{
//o objetivo é centralizar a bolinha antes de seguir em frente
int16 obstaculo = 0;
switch (dados) {
case CEL_DIREITA:
turn_right();
#ifdef DEBUG
printf("cellphone turning right...\n");
#endif
break;
case CEL_ESQUERDA:
turn_left();
#ifdef DEBUG
printf("cellphone turning left...\n");
#endif
break;
case CEL_FRENTE:
#ifdef sensorir
obstaculo = verificaObstaculo();
if (obstaculo == 0) {
#endif
go_forward();
#ifdef DEBUG
printf("no obstacle, go ahead...\n");
#endif
#ifdef sensorir
} else if (obstaculo == 1) {
go_idle();
obstaculo = 0;
#ifdef DEBUG
printf("obstacle, going idle to not crash...\n");
#endif
}
#endif
default:
break;
}
}
int key_press(int keycode, t_env *e)
{
if (keycode == KEY_ALPHA_Q)
move_leftward(e->map, 1);
else if (keycode == KEY_ALPHA_W)
move_forward(e->map, 1);
else if (keycode == KEY_ALPHA_E)
move_rightward(e->map, 1);
else if (keycode == KEY_ALPHA_A)
turn_left(e->map, 1);
else if (keycode == KEY_ALPHA_S)
move_backward(e->map, 1);
else if (keycode == KEY_ALPHA_D)
turn_right(e->map, 1);
return (0);
}
void complete_maze()
{
turn_robot_left();
for (int i = 0; i < 5; i++)
{
back();
while (is_robot_facing_wall())
{
turn_right();
move_robot_forwards();
turn_robot_left();
}
move_robot_forwards();
move_robot_forwards();
}
}
