-
Notifications
You must be signed in to change notification settings - Fork 3
/
Lego.ino
121 lines (97 loc) · 2.25 KB
/
Lego.ino
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
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
#include "analogWrite.h"
#include "LED.h"
#define LEGO_LED 27
#define LEGO_NSLEEP 26
#define LEGO_NFAULT 25
#define LEGO_AIN2 33
#define LEGO_AIN1 32
#define LEGO_BIN2 12
#define LEGO_BIN1 14
bool lego_fault = false;
uint8_t lego_in1[] = { LEGO_AIN1, LEGO_BIN1 };
uint8_t lego_in2[] = { LEGO_AIN2, LEGO_BIN2 };
void lego_motor_disable()
{
digitalWrite(LEGO_NSLEEP, LOW);
analogWrite(LEGO_AIN1, 0, 255);
analogWrite(LEGO_AIN2, 0, 255);
analogWrite(LEGO_BIN1, 0, 255);
analogWrite(LEGO_BIN2, 0, 255);
}
void lego_led(uint8_t state)
{
analogWrite(LEGO_LED, state, 255);
}
void lego_init()
{
lego_motor_disable();
analogWriteFrequency(30000);
analogWriteResolution(8);
pinMode(LEGO_LED, OUTPUT);
digitalWrite(LEGO_LED, HIGH);
pinMode(LEGO_NSLEEP, OUTPUT);
pinMode(LEGO_NFAULT, INPUT_PULLUP);
pinMode(LEGO_AIN1, OUTPUT);
pinMode(LEGO_AIN2, OUTPUT);
pinMode(LEGO_BIN1, OUTPUT);
pinMode(LEGO_BIN2, OUTPUT);
lego_motor(0, 0);
lego_motor(1, 0);
lego_led(0);
delay(10);
digitalWrite(LEGO_NSLEEP, HIGH);
delay(10);
led_anim_red(led_anim_none);
}
void lego_motor(uint8_t motor, uint8_t power, bool forward = true)
{
/* swap motor if configured */
if(current_config.swap)
{
motor = motor ? 0 : 1;
}
/* invert direction */
forward ^= current_config.invert[motor];
if(power == 0)
{
analogWrite(lego_in1[motor], 0, 255);
analogWrite(lego_in2[motor], 0, 255);
}
else
{
uint8_t pwm = min(255.0f, sgn(power) * 0.2f * 255.0f + power * 0.8f);
if(forward)
{
analogWrite(lego_in1[motor], 255 - pwm, 255);
analogWrite(lego_in2[motor], 255, 255);
}
else
{
analogWrite(lego_in1[motor], 255, 255);
analogWrite(lego_in2[motor], 255 - pwm, 255);
}
}
}
void lego_motor(uint8_t motor, float power)
{
lego_motor(motor, min(1.0f,fabsf(power)) * 255, power >= 0);
}
void lego_motor_a(uint8_t motor, float power)
{
lego_motor(0, power);
}
void lego_motor_b(uint8_t motor, float power)
{
lego_motor(1, power);
}
bool lego_loop()
{
if(digitalRead(LEGO_NFAULT) == LOW)
{
Serial.println("[E] DRV8833 reports nFAULT == LOW, disabling");
lego_motor_disable();
lego_fault = true;
led_anim_red(led_anim_emergency);
}
return false;
}