//Begin page main float my_pos[12]; float my_euler[12]; float other_pos[12]; float other_euler[12]; int game_state, inital_approach, hooking, towing,mode; // 0 if initial_approach, 1 if hooking, 2 if towing float length, tow_offset, overshoot_offset; float move_to[3]; void init(){ mode = inital_approach = 0; hooking = 1; towing = 2; game_state = inital_approach; length = 0.3425; tow_offset = -0.007; overshoot_offset = 0.01; } void update() { api.getMyZRState(my_pos); game.getMyEulerState(my_euler); api.getOtherZRState(other_pos); game.getOtherEulerState(other_euler); if (game.getScore() >= 1) game_state = towing; else if (game_state == hooking); // do not change else { if (distance() >= length - overshoot_offset + 0.002) game_state = inital_approach; else game_state = hooking; } } float distance(){ float sum = 0; for (int i = 0; i < 3; i++) sum += mathSquare(my_pos[i] - other_pos[i]); return sqrtf(sum); } void offset_move_to(float offset) { float other_attitude[3] = {other_pos[6], other_pos[7], other_pos[8]}; for (int i = 0; i < 3; i++) move_to[i] = other_attitude[i] * (length - offset) + other_pos[i]; } void tow_fast() { move_to[0] = other_pos[0]; move_to[1] = 1000; move_to[2] = 1000; } void moveTo(float target[3]) { float moveTemp[3]; mathVecSubtract(moveTemp, target, my_pos, 3); float d = mathVecMagnitude(moveTemp, 3); d = sqrt(sqrtf(d * d * d )); for(int i=0;i<3;i++) moveTemp[i]*=(d / mathVecMagnitude(moveTemp, 3)); mathVecAdd(moveTemp, my_pos, moveTemp, 3); api.setPositionTarget(moveTemp); } void align_with_other(int mode) { // Get the roll, pitch, and yaw angles of the other bot and make your bot face it float rpy[3] = {-other_euler[6] + PI, other_euler[7] + PI, other_euler[8]}; if(mode==1){ if(game_state > 0){ for(int i = 0; i < 3; i++) rpy[i] -= other_euler[i+9] * 4.5; rpy[1] *= -1; rpy[1] += PI/128; } else{ for(int i = 0; i < 3; i++) rpy[i] -= other_euler[i+9] * 50; } } game.setEulerTarget(rpy); } void loop(){ update(); DEBUG(("mode: %d, state: %d",mode,game_state)); if(my_pos[1]<0.1) if((other_pos[1]<-0.45 && other_pos[1]>-0.55) && (other_pos[2]>-0.1 && other_pos[2]<0.1) && (other_pos[0]>-0.1 && other_pos[0]<0.1)) mode=1; align_with_other(mode); if (game_state == inital_approach) offset_move_to(overshoot_offset); // overshoot else if (game_state == hooking) offset_move_to(tow_offset+ .005f);// move back else if (game_state == towing) tow_fast(); // tow away if(game_state != towing || game_state == inital_approach) api.setPositionTarget(move_to); else moveTo(move_to); } //End page main