X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_corn.c;h=e64871e2b226e9f24a2833b57d111c04bb9f6b2d;hp=48eff29a45efc24b2f7cf9131d913ac75bb3c2c6;hb=b699727efceb661be65fbd22e975565b800c628e;hpb=a3ec2f79a4ed7b2148ede881e0685ddb6f141d82 diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c index 48eff29..e64871e 100644 --- a/projects/microb2010/mainboard/strat_corn.c +++ b/projects/microb2010/mainboard/strat_corn.c @@ -168,17 +168,15 @@ static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1, return 1; } -/* if (time_get_s() > 32) */ -/* DEBUG(E_USER_STRAT, "i,j = (%d %d), count=%d", i, j, */ -/* corn_count_neigh(i, j)); */ - if (corn_count_neigh(i, j) == 2) return 1; - /* we are on intersection, let's go slow... but as we enter in - * the curve-part of the clitoid, we should not go there */ + + /* we are on intersection, keep the same speed... but as we + * enter in the curve-part of the clitoid, we should not go + * there */ if (wp_belongs_to_line(i, j, num2, dir2)) - return 0; + return clitoid_slow; /* we can ge fast if it's a 60deg angle and if we checked the * current point */ @@ -332,7 +330,6 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1, strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW); } - /* XXX check return value !! */ ret = trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y, line1_a_deg, 150., diff_a_deg, beta_deg, radius, xy_norm(l1.p1.x, l1.p1.y, @@ -350,8 +347,10 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2, reprocess: ret = strat_calc_clitoid(num1, dir1, num2, dir2, &pack_spickles); - if (ret < 0) + if (ret < 0) { DEBUG(E_USER_STRAT, "clitoid failed"); + return END_ERROR; + } /* XXX what to do if cobboard is stucked */ @@ -360,8 +359,13 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2, num2, dir2), flags); + /* error during traj, or traj finished */ + if (err != 0) + return err; + /* the speed has to change */ - if (trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE) + if (err == 0 && + trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE) goto reprocess; DEBUG(E_USER_STRAT, "clitoid started err=%d pack_spickles=%d", @@ -379,13 +383,7 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2, err = wait_traj_end(flags); } - DEBUG(E_USER_STRAT, "clitoid finished"); - - /* XXX 600 -> cste */ - /* XXX does not work, do better */ -/* if (err == 0 && d_speed < 600 && */ -/* mainboard.traj.state == RUNNING_CLITOID_LINE) */ -/* strat_set_speed(600, SPEED_ANGLE_FAST); */ + DEBUG(E_USER_STRAT, "clitoid finished, err=%d", err); strat_rpack60 = 0; strat_lpack60 = 0;