Skip to content

Commit 700229c

Browse files
gbr1gbr1
authored
Merge pull request #5 from gbr1/dev
0.0.3
2 parents f1b840a + 1495d63 commit 700229c

20 files changed

+1332
-50
lines changed

‎.DS_Store‎

6 KB
Binary file not shown.

‎examples/battery/battery.ino‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
#include "Arduino_Robot_Firmware.h"
2+
3+
Arduino_Robot_Firmware robot;
4+
5+
void setup(){
6+
Serial.begin(115200);
7+
robot.begin();
8+
}
9+
10+
void loop(){
11+
robot.updateBMS();
12+
Serial.print(robot.getBatteryVoltage(),4);
13+
Serial.print("\t");
14+
Serial.println(robot.getBatteryChargePercentage(),4);
15+
delay(1000);
16+
}

‎examples/blink/blink.ino‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
#include "Arduino_Robot_Firmware.h"
2+
3+
4+
Arduino_Robot_Firmware robot;
5+
6+
void setup(){
7+
Serial.begin(115200);
8+
robot.begin();
9+
}
10+
11+
void loop(){
12+
robot.setLedBuiltin(HIGH);
13+
delay(100);
14+
robot.setLedBuiltin(LOW);
15+
delay(1000);
16+
}

‎examples/demo_01/demo_01.ino‎

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
#include "Arduino_Robot_Firmware.h"
2+
#include "sensor_line.h"
3+
4+
Arduino_Robot_Firmware robot;
5+
SensorLine line(EXT_A2,EXT_A1,EXT_A0);
6+
7+
void setup() {
8+
robot.begin();
9+
line.begin();
10+
robot.disableIlluminator();
11+
}
12+
13+
void loop() {
14+
robot.updateTouch();
15+
if (robot.getTouchEnter()){
16+
simpleMotors();
17+
}
18+
if (robot.getTouchOk()){
19+
line_follower();
20+
}
21+
}
22+
23+
void simpleMotors(){
24+
int status=0;
25+
unsigned long tmotor=millis();
26+
while(!robot.getTouchDelete()){
27+
switch (status){
28+
case 0:
29+
robot.motor_left->setSpeed(4095);
30+
robot.motor_right->setSpeed(4095);
31+
robot.setLedLeft(COLOR_BLUE);
32+
robot.setLedRight(COLOR_RED);
33+
if (millis()-tmotor>1000){
34+
status++;
35+
tmotor=millis();
36+
}
37+
break;
38+
case 1:
39+
robot.motor_left->setSpeed(4095);
40+
robot.motor_right->setSpeed(-4095);
41+
robot.setLedLeft(COLOR_RED);
42+
robot.setLedRight(COLOR_BLUE);
43+
if (millis()-tmotor>1000){
44+
status++;
45+
status=0;
46+
tmotor=millis();
47+
}
48+
break;
49+
}
50+
robot.updateTouch();
51+
}
52+
robot.motor_left->setSpeed(0);
53+
robot.motor_right->setSpeed(0);
54+
robot.setLeds(COLOR_BLACK);
55+
}
56+
57+
void line_follower(){
58+
float e=0;
59+
float sum_e=0;
60+
float kp = 100.0;
61+
float ki = 0.1;
62+
float control;
63+
unsigned long tline = millis();
64+
65+
while(!robot.getTouchDelete()){
66+
robot.updateTouch();
67+
if (millis()-tline>10){
68+
tline=millis();
69+
line.update();
70+
line.updateCentroid();
71+
control = kp*line.getCentroid();
72+
73+
if ((control<1)&&(control>-1)){
74+
robot.setLedLeft(COLOR_GREEN);
75+
robot.setLedRight(COLOR_GREEN);
76+
}
77+
else{
78+
if (control>=1){
79+
robot.setLedLeft(COLOR_ORANGE);
80+
robot.setLedRight(COLOR_BLACK);
81+
}
82+
else{
83+
robot.setLedLeft(COLOR_BLACK);
84+
robot.setLedRight(COLOR_ORANGE);
85+
}
86+
}
87+
88+
float control_left=2000-control*400;
89+
float control_right=2000+control*400;
90+
91+
if (control_left>4095){
92+
control_left=4095;
93+
}
94+
95+
if (control_left<-4095){
96+
control_left=-4095;
97+
}
98+
99+
if (control_right>4095){
100+
control_right=4095;
101+
}
102+
103+
if (control_right<-4095){
104+
control_right=-4095;
105+
}
106+
107+
robot.motor_left->setSpeed(control_left);
108+
robot.motor_right->setSpeed(control_right);
109+
}
110+
}
111+
robot.motor_left->setSpeed(0);
112+
robot.motor_right->setSpeed(0);
113+
robot.setLeds(COLOR_BLACK);
114+
}

‎examples/demo_02/demo_02.ino‎

Lines changed: 130 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,130 @@
1+
#include "Arduino_Robot_Firmware.h"
2+
#include "sensor_line.h"
3+
4+
Arduino_Robot_Firmware robot;
5+
SensorLine line(EXT_A2,EXT_A1,EXT_A0);
6+
7+
HardwareSerial serial(PA10,PA9);
8+
9+
unsigned long tled = 0;
10+
bool led_value = false;
11+
12+
void setup() {
13+
serial.begin(115200);
14+
robot.begin();
15+
line.begin();
16+
robot.disableIlluminator();
17+
tled = millis();
18+
}
19+
20+
void loop() {
21+
robot.updateTouch();
22+
if (robot.getTouchEnter()){
23+
robot.setLedBuiltin(HIGH);
24+
simpleMotors();
25+
}
26+
if (robot.getTouchOk()){
27+
robot.setLedBuiltin(HIGH);
28+
line_follower();
29+
}
30+
if (millis()-tled>500){
31+
robot.setLedBuiltin(led_value);
32+
led_value=!led_value;
33+
tled=millis();
34+
}
35+
}
36+
37+
void simpleMotors(){
38+
int status=0;
39+
unsigned long tmotor=millis();
40+
while(!robot.getTouchDelete()){
41+
switch (status){
42+
case 0:
43+
robot.motor_left->setSpeed(4095);
44+
robot.motor_right->setSpeed(4095);
45+
robot.setLedLeft(COLOR_BLUE);
46+
robot.setLedRight(COLOR_RED);
47+
if (millis()-tmotor>1000){
48+
status++;
49+
tmotor=millis();
50+
serial.println("forward");
51+
}
52+
break;
53+
case 1:
54+
robot.motor_left->setSpeed(4095);
55+
robot.motor_right->setSpeed(-4095);
56+
robot.setLedLeft(COLOR_RED);
57+
robot.setLedRight(COLOR_BLUE);
58+
if (millis()-tmotor>1000){
59+
status++;
60+
status=0;
61+
tmotor=millis();
62+
serial.println("turn");
63+
}
64+
break;
65+
}
66+
robot.updateTouch();
67+
}
68+
robot.motor_left->setSpeed(0);
69+
robot.motor_right->setSpeed(0);
70+
robot.setLeds(COLOR_BLACK);
71+
}
72+
73+
void line_follower(){
74+
float e=0;
75+
float sum_e=0;
76+
float kp = 100.0;
77+
float ki = 0.1;
78+
float control;
79+
unsigned long tline = millis();
80+
81+
while(!robot.getTouchDelete()){
82+
robot.updateTouch();
83+
if (millis()-tline>10){
84+
tline=millis();
85+
line.update();
86+
line.updateCentroid();
87+
control = kp*line.getCentroid();
88+
serial.println(control);
89+
90+
if ((control<1)&&(control>-1)){
91+
robot.setLeds(COLOR_GREEN);
92+
}
93+
else{
94+
if (control>=1){
95+
robot.setLedLeft(COLOR_ORANGE);
96+
robot.setLedRight(COLOR_BLACK);
97+
}
98+
else{
99+
robot.setLedLeft(COLOR_BLACK);
100+
robot.setLedRight(COLOR_ORANGE);
101+
}
102+
}
103+
104+
float control_left=2000-control*400;
105+
float control_right=2000+control*400;
106+
107+
if (control_left>4095){
108+
control_left=4095;
109+
}
110+
111+
if (control_left<-4095){
112+
control_left=-4095;
113+
}
114+
115+
if (control_right>4095){
116+
control_right=4095;
117+
}
118+
119+
if (control_right<-4095){
120+
control_right=-4095;
121+
}
122+
123+
robot.motor_left->setSpeed(control_left);
124+
robot.motor_right->setSpeed(control_right);
125+
}
126+
}
127+
robot.motor_left->setSpeed(0);
128+
robot.motor_right->setSpeed(0);
129+
robot.setLeds(COLOR_BLACK);
130+
}

‎examples/line/line.ino‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
#include "Arduino_Robot_Firmware.h"
2+
#include "sensor_line.h"
3+
4+
Arduino_Robot_Firmware robot;
5+
SensorLine line(EXT_A2,EXT_A1,EXT_A0);
6+
7+
void setup(){
8+
robot.begin();
9+
line.begin();
10+
Serial.begin(115200);
11+
}
12+
13+
void loop(){
14+
line.update();
15+
line.updateCentroid();
16+
Serial.print(line.getLeft());
17+
Serial.print("\t");
18+
Serial.print(line.getCenter());
19+
Serial.print("\t");
20+
Serial.print(line.getRight());
21+
Serial.print("\t");
22+
Serial.print(line.getCentroid());
23+
Serial.print("\n");
24+
delay(10);
25+
}

‎examples/motor/motor.ino‎

Lines changed: 48 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -1,38 +1,59 @@
11
#include "Arduino_Robot_Firmware.h"
22

3-
Encoder enc_right(TIM5);
3+
Arduino_Robot_Firmware robot;
44

5-
DCmotor motor_right(MOTOR_RIGHT_A,MOTOR_RIGHT_A_CH,MOTOR_RIGHT_B,MOTOR_RIGHT_B_CH);
6-
DCmotor motor_left(MOTOR_LEFT_A,MOTOR_LEFT_A_CH, MOTOR_LEFT_B, MOTOR_LEFT_B_CH);
5+
unsigned long t=0;
6+
unsigned long t_change = 0;
77

8-
//MotorControl right(&motor_right,&enc_right,0,0,0);
8+
int status=0;
9+
float reference = 0.0;
910

10-
void setup(){
11-
motor_right.begin();
12-
motor_left.begin();
11+
void setup(){
12+
Serial.begin(115200);
13+
robot.begin();
14+
t=millis();
15+
t_change=millis();
16+
robot.setRpmRight(reference);
17+
robot.setKPidRight(30.0, 0.1, 0.4);
18+
Serial.print("reference");
19+
Serial.print(" ");
20+
Serial.println("measure");
1321
}
1422

1523
void loop(){
16-
for (int i=0; i<4096; i++){
17-
motor_right.setSpeed(i);
18-
motor_left.setSpeed(i);
19-
delay(1);
24+
if (millis()-t_change>2000){
25+
t_change=millis();
26+
switch (status){
27+
case 0:
28+
reference=0.0;
29+
break;
30+
case 1:
31+
reference=30.0;
32+
break;
33+
case 2:
34+
reference=60.0;
35+
break;
36+
case 3:
37+
reference=-10.0;
38+
break;
39+
case 4:
40+
reference=-60.0;
41+
break;
42+
case 5:
43+
reference=20.0;
44+
break;
2045
}
21-
for (int i=4095; i>0; i--){
22-
motor_right.setSpeed(i);
23-
motor_left.setSpeed(i);
24-
delay(1);
46+
status++;
47+
if (status>1){
48+
status=0;
2549
}
26-
delay(1000);
27-
for (int i=0; i<4096; i++){
28-
motor_right.setSpeed(i);
29-
motor_left.setSpeed(-i);
30-
delay(1);
31-
}
32-
for (int i=4095; i>0; i--){
33-
motor_right.setSpeed(i);
34-
motor_left.setSpeed(-i);
35-
delay(1);
36-
}
37-
delay(1000);
50+
robot.setRpmRight(reference);
51+
}
52+
if (millis()-t>20){
53+
t=millis();
54+
robot.updateMotors();
55+
Serial.print(reference);
56+
Serial.print(" ");
57+
Serial.println(robot.getRpmRight());
58+
}
3859
}

0 commit comments

Comments
 (0)