robot/turn-slow.ic at master · jfriedly/robot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
/**
 * @file turn-slow.ic
 * @brief Code to do many of the same turns as in turn.ic just slower so that we can keep the wagon attached.
 * @author Andrew Krieger and Joel Friedly
 * @date 5/23/2011
 */

#use "defines.ic"
#use "drive.ic"
#use "servo/exp_servo_lib.ic"
#use "servo/exp_servo_calibrate.ic"
#use "CdS.ic"
#use "line.ic"
#use "turn-slow.ic"
#use "turn.ic"
#use "step.ic"
#use "gps.ic"
#use "line.ic"


/**
 * @brief Turn by deg, relative to current position
 * @param deg Degrees to turn.  Positive is CCW, negative is CW.
 *
 * This will repeatedly call [c]cw_degrees to turn.  This turns with less motor power
 * than the regular @ref turn function.
 */
void turn_slow(float deg) {
    if(deg > 0.0) {
        while(deg > 45.0) {
            ccw_degrees_slow(45.0);
            deg -= 45.0;
        }

        ccw_degrees_slow(deg);
    } else if(deg < 0.0) {
        while(deg < -45.0) {
            cw_degrees_slow(45.0);
            deg += 45.0;
        }

        cw_degrees_slow(-deg);
    }
}

/**
 * @brief Helper function to slowly turn counterclockwise
 * @param deg Number of degrees to turn (must be nonnegative)
 */
void ccw_degrees_slow(float deg) {
    int left_trans = round(deg * (92.5 / 360.0));
    int right_trans = round(deg * (92.5 / 360.0));
    int diff_l, diff_r;

    //    printf("Turning %f deg\n", deg);

    SHAFT_LEFT_COUNT = SHAFT_RIGHT_COUNT = 0;
    reset_system_time();

    while(mseconds() < 2000L)
      {
        diff_l = SHAFT_LEFT_COUNT - left_trans;
        diff_r = SHAFT_RIGHT_COUNT - right_trans;

        if(diff_l + diff_r > 1)
          break;

        motor(MOTOR_LEFT, -70);
        motor(MOTOR_RIGHT, 70);

    }

    ao();
}

/**
 * @brief Helper function to slowly turn clockwise.
 * @param deg Number of degrees to turn (must be nonnegative)
 */
void cw_degrees_slow(float deg) {

    int left_trans = round(deg * (92.5 / 360.0));
    int right_trans = round(deg * (92.5 / 360.0));
    int disp_l, disp_r;
    int diff_l, diff_r;

    reset_system_time();
    SHAFT_LEFT_COUNT = SHAFT_RIGHT_COUNT = 0;

    while(mseconds() < 2000L)
      {
        diff_l = SHAFT_LEFT_COUNT - left_trans;
        diff_r = SHAFT_RIGHT_COUNT - right_trans;

        if(diff_l + diff_r > 1)
          break;

        motor(MOTOR_LEFT, 70);
        motor(MOTOR_RIGHT, -70);

    }

    ao();
}