utils.cpp 4.53 KB
Newer Older
mattdr's avatar
mattdr committed
1
2
3
4
5
#include <Arduino.h>
#include "utils.h"
#include "motor_ctrl.h"
#include "encoder.h"

6
7
8
const float UPWARD_ROC_LIMITER = 0.005;
float UPWARD_ROC_PREV_VALUE = 0;

mattdr's avatar
mattdr committed
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
//This function will just block until the user hits the space bar
void block_until_spacebar(void)
{
    short incomingByte = 0;
    while(1)
    {
        Serial.println("Press Space to continue");  
        delay(200);  // do not go too fast!
        if (Serial.available()) {
            incomingByte = Serial.read();
        }
        if(incomingByte == 32)
        {
            return;
        }
    }
}

//reads potentiometer on pin A1 and scales it 0-100
//The value is inverted so the value is 100-0
int get_pot_1_inv(void)
{
    volatile float potval = analogRead(A1);
    potval = potval/1024;
    potval = 1 - potval;
    potval = potval * 100;
    return (int)potval; //read from pot
}

//reads potentiometer on pin A0 and scales by scale_factor
//NOTE: scale factor should only be 0-255!
// TODO: make sure this code is not run too often it will take time
int get_pot_0_scaled(int scaleFactor, float period_over_255)
{
    volatile float raw_potval = analogRead(A0);
    float potval = raw_potval;
45
46
47
48
49
50
51
52
53
54

    if ((potval - UPWARD_ROC_PREV_VALUE) > UPWARD_ROC_LIMITER) {
        potval = UPWARD_ROC_PREV_VALUE + UPWARD_ROC_LIMITER;
    }
    else if ((potval - UPWARD_ROC_PREV_VALUE) < UPWARD_ROC_LIMITER) {
        potval = UPWARD_ROC_PREV_VALUE - UPWARD_ROC_LIMITER;
    }

    UPWARD_ROC_PREV_VALUE = potval;

mattdr's avatar
mattdr committed
55
56
57
58
59
60
61
62
63
64
65
66
67
68
    potval = potval/1024;
    potval = potval * scaleFactor; 

    potval -= 3.0; // this just disables the first three pot ticks

    // This code is used to make sure that the pwm nver has a period
    // lower than 250 ns (as this may be problematic for gate drivrs)
    if ((period_over_255 * potval) < 250.0E-9){
        return (int)0;
    }

    return (int)potval;
}

69
//this just prints the output of get_pot_0_scaled
70
int debug_pot_0_scaled()
71
72
73
{
    while(1){
        Serial.println("value from get_pot_0_scaled");  
74
        Serial.println(get_pot_0_scaled(240,0.0000000817));
75
76
77
78
79
80
81
        delay(200);  // do not go too fast!

    }

}


mattdr's avatar
mattdr committed
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
// This function just rotates the motor to position CB, waits until
// everything stops moving, takes a read of what the encoder has
// prints it to the command line and then turns everything off
void do_hard_coded_offset(void)
{
    int rtn = calibrate_encoder();
    while(1)
    {
        int my_position = 0;
        set_block_phase(6);
        delayMicroseconds(1000000); //1 sec delay
        //now lets grab the position 
        my_position = get_raw_wheel_position_ticks() - get_Zoffset();
        //turn it off!
        turn_all_off();
        //print it out wayyyy too much It is supposed to get stuck here
        Serial.println("Get Hard coded offset got a BC position of:");
        Serial.println(my_position);
        Serial.println("HARD CODE THIS AND REBOOT!!!");
        Serial.println("press space to try again");
        delayMicroseconds(100000); //0.1 sec delay
        block_until_spacebar();

    }
}

//Locate a state, hold for half a second, print encoder value, 
//repeat for each state
void locate_state_positions(void) 
{
112
    //calibrate_encoder();
mattdr's avatar
mattdr committed
113
    block_until_spacebar();
114
    set_block_phase(1);
mattdr's avatar
mattdr committed
115
116
117
118
119
120
121
122
123
124
125
126
127
128

    for (int i = 0; i < 12; i++) 
    {
        for(int p = 1; p <= 6; p++)
        {
            set_block_phase(p);
            delayMicroseconds(500000/5); //delay half-a-second
            Serial.printf("%d:%d\n", p, get_wheel_position_ticks());
        }
    }
    turn_all_off();
    while(1)
    {
    }
129
130
131
132
}

void debug_low_state_by_state(void)
{
133
134

    while(1){
135
136
137
138
    Serial.println("lets run some debug tests");
    Serial.println("Hit space to call turn_all_off()");
    block_until_spacebar();
    turn_all_off();
139
140
141
    Serial.println("Hit space to call setphase(1,0,0,0,0,0");
    block_until_spacebar();
    set_phase(1,0,0,0,0,0);
142
143
144
    Serial.println("Hit space to call setphase(0,0,0,0,0,0)");
    block_until_spacebar();
    set_phase(0,0,0,0,0,0);
thcl's avatar
thcl committed
145
    Serial.println("Hit space to call setphase(0,0,0,0,0,1)");
146
    block_until_spacebar();
thcl's avatar
thcl committed
147
    set_phase(0,0,0,0,0,1);
148
149
150
    Serial.println("Hit space to call setphase(0,0,0,0,1,0)");
    block_until_spacebar();
    set_phase(0,0,0,0,1,0);
thcl's avatar
thcl committed
151
    Serial.println("Hit space to call setphase(0,0,0,1,0,0)");
152
    block_until_spacebar();
thcl's avatar
thcl committed
153
    set_phase(0,0,0,1,0,0);
154
155
156
    Serial.println("Hit space to call setphase(0,0,0,1,1,1)");
    block_until_spacebar();
    set_phase(0,0,0,1,1,1);
157
158
159
160
161
    // Serial.println("Thats all i got restart board!!!")
    Serial.println("Digital Write");
    digitalWrite(5, HIGH);
    block_until_spacebar();
    }
mattdr's avatar
mattdr committed
162
}