Util.cpp 4.81 KB
Newer Older
Werner Sembach's avatar
Cleanup  
Werner Sembach committed
1
2
3
4
5
6
7
8
9
10
11
12
13
#include "Util.h"

#include <math.h>

#include "stm32f411e_discovery.h"
#include "stm32f411e_discovery_gyroscope.h"
#include "stm32f411e_discovery_accelerometer.h"

#include "ezs_io.h"

#include "ADC.h"
#include "DCMotor.h"
#include "ExternButton.h"
Margit Haspel's avatar
Margit Haspel committed
14
#include "LeverKey.h"
Werner Sembach's avatar
Cleanup  
Werner Sembach committed
15
16
#include "ServoMotor.h"

Margit Haspel's avatar
Margit Haspel committed
17
18
19
20
21
22
23
24
25
extern "C" struct spring set_spring(int threshold, char direction, int power, bool solved){
    struct spring set;
    set.threshold = threshold;
    set.direction = direction;
    set.power = power;
    set.solved = solved;
    return set;
}

Werner Sembach's avatar
Cleanup  
Werner Sembach committed
26
27
28
29
extern "C" void all_init() {
    ezs_printf("\n\n================================\n");
    ezs_printf("Start Init\n\n");

Werner Sembach's avatar
Werner Sembach committed
30
31
32
33
34
35
36
37
38
    adc_init();
    ezs_printf("ADC loaded\n");

    dc_init();
    ezs_printf("DC loaded\n");

    button_init();
    ezs_printf("Extern Button loaded\n");

Margit Haspel's avatar
Margit Haspel committed
39
40
41
    lever_init();
    ezs_printf("Levers loaded\n");

Werner Sembach's avatar
Werner Sembach committed
42
43
44
    servo_init();
    ezs_printf("Servo loaded\n");

Werner Sembach's avatar
Cleanup  
Werner Sembach committed
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
    BSP_PB_Init(BUTTON_KEY, BUTTON_MODE_GPIO);
    ezs_printf("Button loaded\n");

    uint8_t errno_gyro = BSP_GYRO_Init();
    if (errno_gyro != GYRO_OK) {
        ezs_printf("Gyro Error!\n");
        return;
    }
    ezs_printf("Gyro OK\n");

    uint8_t errno_accelero = BSP_ACCELERO_Init();
    if (errno_accelero != ACCELERO_OK) {
        ezs_printf("Accelero Error!\n");
        return;
    }
    ezs_printf("Accelero OK\n");

    ezs_printf("\nFinish Init\n");
    ezs_printf("================================\n\n");
}

Werner Sembach's avatar
Werner Sembach committed
66
//debug output
Werner Sembach's avatar
Cleanup  
Werner Sembach committed
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
extern "C" void print_gyro() {
    float xyz_gyro[3];
    BSP_GYRO_GetXYZ(xyz_gyro);
    ezs_printf("Gyro:                  %12.1f, %12.1f, %12.1f\n", xyz_gyro[0], xyz_gyro[1], xyz_gyro[2]);
}

extern "C" void print_accelero() {
    int16_t xyz_accelero[3];
    BSP_ACCELERO_GetXYZ(xyz_accelero);
    ezs_printf("Accelero:              %12d, %12d, %12d\n", xyz_accelero[0], xyz_accelero[1], xyz_accelero[2]);
}

extern "C" void print_degree() {
    ezs_printf("Degree Short/Long/Max: %12.1f, %12.1f, %12.1f\n", get_degree_short(), get_degree_long(), get_degree_max());
}

Margit Haspel's avatar
Margit Haspel committed
83
84
85
86
extern "C" void print_levers() {
    ezs_printf("Levers: 0: %d, 1: %d, 2: %d, 3: %d\n", lever_get(0), lever_get(1), lever_get(2), lever_get(3));
}

Werner Sembach's avatar
Werner Sembach committed
87
//get tilt in degrees
Werner Sembach's avatar
Cleanup  
Werner Sembach committed
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
extern "C" double get_degree_short() {
    int16_t xyz_accelero[3];
    BSP_ACCELERO_GetXYZ(xyz_accelero);

    return atan2(xyz_accelero[1], xyz_accelero[2]) / M_PI * 180.;
}

extern "C" double get_degree_long() {
    int16_t xyz_accelero[3];
    BSP_ACCELERO_GetXYZ(xyz_accelero);

    return atan2(xyz_accelero[0], xyz_accelero[2]) / M_PI * 180.;
}

extern "C" double get_degree_max() {
    double neigung_zu_kurz_deg = fabs(get_degree_short());
    double neigung_zu_lang_deg = fabs(get_degree_long());

    if (neigung_zu_kurz_deg > neigung_zu_lang_deg) {
        return fabs(neigung_zu_kurz_deg);
    }
    return fabs(neigung_zu_lang_deg);
}

Margit Haspel's avatar
Margit Haspel committed
112
113
114
115
116
117
118
119
//get all levers at once in an array
extern "C" void get_all_levers(int levers[]) {
    levers[0] = lever_get(0);
    levers[1] = lever_get(1);
    levers[2] = lever_get(2);
    levers[3] = lever_get(3);
}

Margit Haspel's avatar
Margit Haspel committed
120
121
122
123
124
125
126
127
/*returns true if spring is in "broken" state
gets an array of structs that contain:
int threshold - if the poti gives back a value smaller than this
char direction - motor turn in this direction (l, r, anything else)
int power - with this power (-1 sets the power to be linear to the adc value)
bool solved - what is returned if the threshold is reached
*/
extern "C" bool simulate_spring(struct spring springs[], int len) {
Werner Sembach's avatar
Cleanup  
Werner Sembach committed
128
129
    uint16_t adc_value = adc_get(11);

Margit Haspel's avatar
Margit Haspel committed
130
    for(int i = 0; i < len; i++){
Werner Sembach's avatar
Cleanup    
Werner Sembach committed
131
        //ezs_printf("%d\n", adc_value);
Margit Haspel's avatar
Margit Haspel committed
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
        if (adc_value < springs[i].threshold){
            if (springs[i].power < 0){
                if (springs[i].direction == 'l'){
                    dc_set(DC_LEFT, adc_value / 4096.0 * 255);
                } else if (springs[i].direction == 'r'){
                    dc_set(DC_RIGHT, adc_value / 4096.0 * 255);
                } else {
                    dc_set(0,0);
                }
            }
            else {
                if (springs[i].direction == 'l'){
                    dc_set(DC_LEFT, springs[i].power);
                } else if (springs[i].direction == 'r'){
                    dc_set(DC_RIGHT, springs[i].power);
                } else {
                    dc_set(0,0);
                }
            }
            return springs[i].solved;
        }
    }
    // in case no threshold is "reached" (e.g. the struct params are faulty)
    // the box shouldn't be unopenable then
    return true;
Werner Sembach's avatar
Cleanup  
Werner Sembach committed
157
158
}

Margit Haspel's avatar
Margit Haspel committed
159
160
// turns the motor real fast if above certain position
extern "C" void block_spring(int pos) {
Werner Sembach's avatar
Cleanup  
Werner Sembach committed
161
    uint16_t adc_value = adc_get(11);
Margit Haspel's avatar
Margit Haspel committed
162
    if(adc_value > pos) {
Werner Sembach's avatar
Cleanup  
Werner Sembach committed
163
164
        dc_set(DC_RIGHT, 255);
    }
Margit Haspel's avatar
Margit Haspel committed
165
166
    else if(adc_value > (pos - 256)) {
        uint8_t speed = adc_value - (pos - 256) / 256.0 * 255;
Werner Sembach's avatar
Cleanup  
Werner Sembach committed
167
168
169
170
171
172
        dc_set(DC_RIGHT, speed);
    }
    else {
        dc_set(0, 0);
    }
}