Skip to content

Commit aae1ebe

Browse files
wxy6655wxy6655
committed
update new firmware
can use maincontrol and calibration ，transponder，information
1 parent 0fe6ca5 commit aae1ebe

File tree

10 files changed

+1400
-0
lines changed

10 files changed

+1400
-0
lines changed

‎Arduino/MycobotBasic/examples/miniRoboFlow/Calibration.cpp

Lines changed: 168 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,168 @@
1+
#include "Calibration.h"
2+
3+
int calibrate_servo_no = 1;
4+
5+
void Calibration::bration(MycobotBasic &myCobot){
6+
M5.Lcd.setTextSize(2);
7+
// EEPROM.begin(EEPROM_SIZE);//new
8+
myCobot.setLEDRGB(255, 255, 255);
9+
Calibration::info();
10+
while (1)
11+
{
12+
M5.update(); // need to call update()
13+
M5.Lcd.setCursor(0,0);
14+
if (M5.BtnA.wasPressed()) {
15+
myCobot.setLEDRGB(255, 0, 0);
16+
Calibration::init(myCobot);
17+
}
18+
if (M5.BtnB.wasPressed()) {
19+
myCobot.setLEDRGB(0, 255, 0);
20+
Calibration::test(myCobot);
21+
}
22+
if (M5.BtnC.wasReleasefor(1000)) {
23+
myCobot.setLEDRGB(0, 0, 255);
24+
Calibration::reset(myCobot);
25+
break;
26+
}
27+
}
28+
}
29+
30+
void Calibration::info()
31+
{
32+
M5.Lcd.clear(BLACK);
33+
M5.Lcd.setTextColor(BLACK);
34+
M5.Lcd.setTextColor(RED);
35+
M5.Lcd.setTextSize(3);
36+
M5.Lcd.setCursor(0, 10);
37+
M5.Lcd.printf("myCobot");
38+
M5.Lcd.setCursor(0, 40);
39+
M5.Lcd.drawFastHLine(0,70,320,GREY);
40+
M5.Lcd.setTextSize(3);
41+
M5.Lcd.println("Basic Calibration");
42+
M5.Lcd.setTextSize(2);
43+
M5.Lcd.setTextColor(WHITE);
44+
M5.Lcd.setCursor(0, 100);
45+
delay(50);
46+
if(lan == 2){
47+
M5.Lcd.drawString("按A键 - 设置舵机零位", 20, 40, 1);
48+
M5.Lcd.drawString("按B键 - 测试舵机", 20, 80, 1);
49+
M5.Lcd.drawString("按C键 - 退出此程序",20, 120, 1);
50+
}
51+
if(lan == 1){
52+
M5.Lcd.println("Press A - Calibrate Servo ");
53+
M5.Lcd.println();
54+
// M5.Lcd.print("PressB - Test Servos (long press to force testing)\n\n");
55+
M5.Lcd.println("Press B - Test Servos ");
56+
M5.Lcd.println();
57+
M5.Lcd.println("Press C - Exit(1S)\n");
58+
// M5.Lcd.setCursor(0, 170);
59+
// M5.Lcd.print("(long press to return language selection)\n");
60+
}
61+
M5.update();
62+
}
63+
64+
void Calibration::init(MycobotBasic &myCobot)
65+
{
66+
M5.Lcd.clear(BLACK);
67+
delay(50);
68+
69+
if (calibrate_servo_no>6)
70+
{
71+
if(lan == 2){
72+
M5.Lcd.drawString("已经设置好所有舵机", 20, 20, 1);
73+
}
74+
if(lan == 1){
75+
M5.Lcd.setCursor(0, 30);
76+
M5.Lcd.print("Already Calibrate all \n");
77+
}
78+
delay(500);
79+
Calibration::info();
80+
return;
81+
}
82+
83+
myCobot.setServoCalibration(calibrate_servo_no);
84+
85+
if(lan == 2){
86+
M5.Lcd.drawString("已设置舵机", 20, 20, 1);
87+
M5.Lcd.setTextSize(2);
88+
M5.Lcd.setCursor(0, 150);
89+
M5.Lcd.printf("%d",calibrate_servo_no);
90+
M5.Lcd.setTextSize(2);
91+
}
92+
if(lan == 1){
93+
M5.Lcd.setTextSize(2);
94+
M5.Lcd.setCursor(0, 30);
95+
M5.Lcd.print("Calibrating\nServo\n\n");
96+
M5.Lcd.setTextSize(6);
97+
M5.Lcd.setCursor(30, 100);
98+
M5.Lcd.printf("%d",calibrate_servo_no);
99+
M5.Lcd.setTextSize(2);
100+
}
101+
102+
delay(100);
103+
104+
myCobot.setEncoder(calibrate_servo_no, 2047);
105+
delay(400);
106+
107+
calibrate_servo_no ++;
108+
}
109+
110+
void Calibration::test(MycobotBasic &myCobot)
111+
{
112+
M5.Lcd.clear(BLACK);
113+
delay(50);
114+
// move all servos
115+
if (calibrate_servo_no >= 6)
116+
{
117+
for (int i = 1; i < 7; i ++)
118+
{
119+
if(lan == 2){
120+
M5.Lcd.drawString("已设置舵机零位 ", 20, 20, 1);
121+
}
122+
if(lan == 1){
123+
M5.Lcd.setCursor(20, 20 + 20*i);
124+
M5.Lcd.print("Move servo -> ");
125+
M5.Lcd.println(i);
126+
}
127+
128+
myCobot.setEncoder(i, 1848);
129+
delay(2500);
130+
myCobot.setEncoder(i, 2248);
131+
delay(3000);
132+
myCobot.setEncoder(i, 2048);
133+
delay(2500);
134+
}
135+
Calibration::info();
136+
delay(2000);
137+
}
138+
else{
139+
if(lan == 2){
140+
M5.Lcd.drawString("请先设定关节零位", 20, 20, 1);
141+
}
142+
if(lan == 1){
143+
M5.Lcd.setCursor(0, 30);
144+
M5.Lcd.print("Only move after all servo calibrated");
145+
}
146+
delay(2000);
147+
Calibration::info();
148+
return;
149+
}
150+
}
151+
152+
void Calibration::reset(MycobotBasic &myCobot)
153+
{
154+
M5.Lcd.clear(BLACK);
155+
delay(50);
156+
if(lan == 2){
157+
M5.Lcd.drawString("重新设置", 20, 20, 1);
158+
}
159+
if(lan == 1){
160+
M5.Lcd.setCursor(0, 30);
161+
M5.Lcd.print("Restart to calibrate");
162+
}
163+
calibrate_servo_no = 0;
164+
//关闭扭力输出
165+
myCobot.setFreeMove();
166+
delay(1000);
167+
Calibration::info();
168+
}

‎Arduino/MycobotBasic/examples/miniRoboFlow/Calibration.h

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
#ifndef Calibration_h
2+
#define Calibration_h
3+
4+
#include <MycobotBasic.h>
5+
#include "config.h"
6+
7+
8+
9+
class Calibration
10+
{
11+
private:
12+
void info();
13+
void init(MycobotBasic &myCobot);
14+
void test(MycobotBasic &myCobot);
15+
void reset(MycobotBasic &myCobot);
16+
public:
17+
void bration(MycobotBasic &myCobot);
18+
};
19+
20+
#endif

‎Arduino/MycobotBasic/examples/miniRoboFlow/Information.cpp

Lines changed: 101 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
#include "Information.h"
2+
3+
SMSBL sm;
4+
5+
void Connect::info(){
6+
M5.Lcd.clear(BLACK);
7+
M5.Lcd.setTextColor(BLACK);
8+
M5.Lcd.setTextColor(RED);
9+
M5.Lcd.setTextSize(3);
10+
M5.Lcd.setCursor(0, 10);
11+
M5.Lcd.printf("myCobot");
12+
M5.Lcd.setCursor(0, 40);
13+
M5.Lcd.drawFastHLine(0,70,320,GREY);
14+
M5.Lcd.setTextSize(3);
15+
M5.Lcd.println("Status Inspection");
16+
M5.Lcd.setCursor(0, 100);
17+
M5.Lcd.setTextColor(WHITE);
18+
M5.Lcd.setTextSize(2);
19+
M5.Lcd.println("Press A - Servos motor");
20+
M5.Lcd.println();
21+
M5.Lcd.println("Press B - Firmware");
22+
M5.Lcd.println();
23+
M5.Lcd.println("Press C - Exit(1S)");
24+
}
25+
26+
void Connect::test(MycobotBasic &myCobot){
27+
info();
28+
sm.pSerial = &Serial2;
29+
while (1)
30+
{
31+
// put your main code here, to run repeatedly:
32+
M5.update(); // need to call update()
33+
M5.Lcd.setCursor(0,0);
34+
// M5.Lcd.clear(BLACK);
35+
36+
if (M5.BtnA.wasReleased()) {
37+
Connect::testServo(myCobot);
38+
}
39+
if (M5.BtnB.wasReleased()) {
40+
Connect::ReadConfig();
41+
}
42+
if (M5.BtnC.wasReleasefor(1000)) {
43+
break;
44+
}
45+
}
46+
}
47+
48+
void Connect::testServo(MycobotBasic &myCobot){
49+
M5.Lcd.clear(BLACK);
50+
delay(50);
51+
M5.Lcd.setTextSize(3);
52+
M5.Lcd.setCursor(0, 10);
53+
M5.Lcd.setTextColor(WHITE);
54+
M5.Lcd.println("connect test");
55+
M5.Lcd.setTextSize(2);
56+
M5.Lcd.setCursor(0, 40);
57+
int state = myCobot.isPoweredOn();
58+
M5.Lcd.print("atom - ");
59+
if(state == 1){
60+
M5.Lcd.setTextColor(GREEN);
61+
M5.Lcd.println("ok");
62+
}else{
63+
M5.Lcd.setTextColor(RED);
64+
M5.Lcd.println("no");
65+
}
66+
M5.Lcd.setTextSize(2);
67+
M5.Lcd.setCursor(0, 60);
68+
M5.Lcd.setTextColor(WHITE);
69+
for(int i = 1; i<7;i++){
70+
M5.Lcd.setTextColor(WHITE);
71+
M5.Lcd.print("servo ");
72+
M5.Lcd.print(i);
73+
M5.Lcd.print(" - ");
74+
if(sm.FeedBack(i) != -1){
75+
M5.Lcd.setTextColor(GREEN);
76+
M5.Lcd.println("ok");
77+
}else{
78+
M5.Lcd.setTextColor(RED);
79+
M5.Lcd.println("no");
80+
}
81+
delay(50);
82+
}
83+
M5.update();
84+
delay(3000);
85+
info();
86+
}
87+
88+
89+
void Connect::ReadConfig(){
90+
M5.Lcd.clear(BLACK);
91+
delay(50);
92+
M5.Lcd.setTextSize(3);
93+
M5.Lcd.setCursor(0, 10);
94+
M5.Lcd.println("connect test");
95+
M5.Lcd.setTextSize(2);
96+
M5.Lcd.setCursor(0, 40);
97+
M5.Lcd.println("Waiting for development");
98+
M5.update();
99+
delay(3000);
100+
info();
101+
}

‎Arduino/MycobotBasic/examples/miniRoboFlow/Information.h

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
#ifndef Information_h
2+
#define Information_h
3+
4+
#include "MycobotBasic.h"
5+
#include "SCServo.h"
6+
#include "config.h"
7+
8+
class Connect
9+
{
10+
private:
11+
/* data */
12+
void info();
13+
void testServo(MycobotBasic &myCobot);
14+
void ReadConfig();
15+
public:
16+
void test(MycobotBasic &myCobot);
17+
};
18+
19+
#endif

0 commit comments

Comments
 (0)