-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.cpp
154 lines (138 loc) · 3.13 KB
/
main.cpp
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
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
#include "mbed.h"
#include "MakisumiACMotor.h"
#include "AS5600.h"
#if defined(L1)
#define ID 'a'
#define OFFSET -0.985056
#elif defined(L2)
#define ID 'b'
#define OFFSET 0.549299
#elif defined(R1)
#define ID 'c'
#define OFFSET -1.360973
#elif defined(R2)
#define ID 'd'
#define OFFSET 2.458037
#else
#define ID 'a'
#define OFFSET 0.0
#endif
#define MAX_ANGLE 30
#define MIN_ANGLE -30
#define GAIN 10.0
#define BAUDRATE 57600
#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif
#define max(a, b) ((a) > (b) ? (a) : (b))
#define min(a, b) ((a) < (b) ? (a) : (b))
BusOut led(LED2, LED3, LED4);
BusIn sw(SW1, SW2);
MakisumiACMotor acmotor(LED1);
AS5600 as5600(I2C_SDA, I2C_SCL);
Serial serial(USBTX, USBRX);
Timer t;
float target_angle = MIN_ANGLE;
float dir = 0.1;
bool is_servo_on = false;
bool change_target = false;
Ticker changeDistAngleTicker;
void initialize()
{
LPC_IOCON -> SWCLK_PIO0_10 |= 0x01;
LPC_IOCON -> SWDIO_PIO1_3 |= 0x01;
}
void isrRx() {
static char buf[10];
static int n = 0;
while(serial.readable()){
buf[n ++] = serial.getc();
if (n >= 10) n = 0;
if (buf[n-1] == '\r'){
int t_angle;
float f_angle;
n = 0;
if (buf[0] != ID) return;
if (strlen(&buf[1] )!= 5) return;
sscanf(&buf[1], "%d", &t_angle);
f_angle = (float)t_angle / 10.0f;
// printf("%d\r\n", t_angle);
memset(buf, 0, sizeof(buf));
if ((f_angle > MAX_ANGLE) || (f_angle < MIN_ANGLE)) return;
target_angle = f_angle * M_PI / 180.0f;
if (is_servo_on == false){
acmotor.servoOn();
is_servo_on = true;
}
change_target = true;
}
}
}
void changeDistAngle() {
target_angle += dir;
if ((target_angle >= MAX_ANGLE) || (target_angle <= MIN_ANGLE)){
target_angle *= -1.0;
}
}
int main() {
int previous_hole_state = 6;
float gain = GAIN, max_value = 1.0;
int counter = 25, led_counter = 100, led_state = 0;
bool is_low_gain = true;
initialize();
led = 0;
sw.mode(PullUp);
serial.attach(isrRx, Serial::RxIrq);
serial.baud(BAUDRATE);
changeDistAngleTicker.attach(&changeDistAngle, 0.01);
as5600 = as5600 - OFFSET;
while(1){
float angle = as5600;
// printf("%f\r\n", angle);
if (as5600.getError()) break;
if (is_low_gain){
if(fabs(angle - target_angle) < 0.1){
acmotor.setMaxDutyRatio(0.5);
is_low_gain = false;
}
}
float val = max(min(gain * (angle - target_angle), max_value), -max_value);
#if defined(L1) || defined(L2) || defined(R1) || defined(R2)
acmotor = -val;
#else
acmotor = val;
#endif
if (change_target){
acmotor.status_changed();
change_target = false;
}
if (sw[0] == 0 && counter == 0){
target_angle += 0.1;
counter = 25;
}
if (sw[1] == 0 && counter == 0){
target_angle -= 0.1;
counter = 25;
}
if (counter > 0) counter --;
if (led_counter == 0){
led_state ^= 1;
led[0] = led_state;
led_counter = 100;
}
led_counter --;
t.reset();
t.start();
while(t.read() < 0.020f){
if (acmotor.getHoleState() != previous_hole_state){
acmotor.status_changed();
previous_hole_state = acmotor.getHoleState();
// printf("%d\r\n", previous_hole_state);
}
}
}
acmotor = 0;
acmotor.status_changed();
led = 0x02;
while(1);
}