Skip to content

Commit

Permalink
update new firmware
Browse files Browse the repository at this point in the history 
can use maincontrol and calibration ，transponder，information
  • Loading branch information
@wxy6655
wxy6655 committed Jul 30, 2021
1 parent 0fe6ca5 commit aae1ebe
Show file tree
Hide file tree
Showing 10 changed files with 1,400 additions and 0 deletions.
168 changes: 168 additions & 0 deletions Arduino/MycobotBasic/examples/miniRoboFlow/Calibration.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,168 @@
#include "Calibration.h"

int calibrate_servo_no = 1;

void Calibration::bration(MycobotBasic &myCobot){
M5.Lcd.setTextSize(2);
// EEPROM.begin(EEPROM_SIZE);//new
myCobot.setLEDRGB(255, 255, 255);
Calibration::info();
while (1)
{
M5.update(); // need to call update()
M5.Lcd.setCursor(0,0);
if (M5.BtnA.wasPressed()) {
myCobot.setLEDRGB(255, 0, 0);
Calibration::init(myCobot);
}
if (M5.BtnB.wasPressed()) {
myCobot.setLEDRGB(0, 255, 0);
Calibration::test(myCobot);
}
if (M5.BtnC.wasReleasefor(1000)) {
myCobot.setLEDRGB(0, 0, 255);
Calibration::reset(myCobot);
break;
}
}
}

void Calibration::info()
{
M5.Lcd.clear(BLACK);
M5.Lcd.setTextColor(BLACK);
M5.Lcd.setTextColor(RED);
M5.Lcd.setTextSize(3);
M5.Lcd.setCursor(0, 10);
M5.Lcd.printf("myCobot");
M5.Lcd.setCursor(0, 40);
M5.Lcd.drawFastHLine(0,70,320,GREY);
M5.Lcd.setTextSize(3);
M5.Lcd.println("Basic Calibration");
M5.Lcd.setTextSize(2);
M5.Lcd.setTextColor(WHITE);
M5.Lcd.setCursor(0, 100);
delay(50);
if(lan == 2){
M5.Lcd.drawString("按A键 - 设置舵机零位", 20, 40, 1);
M5.Lcd.drawString("按B键 - 测试舵机", 20, 80, 1);
M5.Lcd.drawString("按C键 - 退出此程序",20, 120, 1);
}
if(lan == 1){
M5.Lcd.println("Press A - Calibrate Servo ");
M5.Lcd.println();
// M5.Lcd.print("PressB - Test Servos (long press to force testing)\n\n");
M5.Lcd.println("Press B - Test Servos ");
M5.Lcd.println();
M5.Lcd.println("Press C - Exit(1S)\n");
// M5.Lcd.setCursor(0, 170);
// M5.Lcd.print("(long press to return language selection)\n");
}
M5.update();
}

void Calibration::init(MycobotBasic &myCobot)
{
M5.Lcd.clear(BLACK);
delay(50);

if (calibrate_servo_no>6)
{
if(lan == 2){
M5.Lcd.drawString("已经设置好所有舵机", 20, 20, 1);
}
if(lan == 1){
M5.Lcd.setCursor(0, 30);
M5.Lcd.print("Already Calibrate all \n");
}
delay(500);
Calibration::info();
return;
}

myCobot.setServoCalibration(calibrate_servo_no);

if(lan == 2){
M5.Lcd.drawString("已设置舵机", 20, 20, 1);
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(0, 150);
M5.Lcd.printf("%d",calibrate_servo_no);
M5.Lcd.setTextSize(2);
}
if(lan == 1){
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(0, 30);
M5.Lcd.print("Calibrating\nServo\n\n");
M5.Lcd.setTextSize(6);
M5.Lcd.setCursor(30, 100);
M5.Lcd.printf("%d",calibrate_servo_no);
M5.Lcd.setTextSize(2);
}

delay(100);

myCobot.setEncoder(calibrate_servo_no, 2047);
delay(400);

calibrate_servo_no ++;
}

void Calibration::test(MycobotBasic &myCobot)
{
M5.Lcd.clear(BLACK);
delay(50);
// move all servos
if (calibrate_servo_no >= 6)
{
for (int i = 1; i < 7; i ++)
{
if(lan == 2){
M5.Lcd.drawString("已设置舵机零位 ", 20, 20, 1);
}
if(lan == 1){
M5.Lcd.setCursor(20, 20 + 20*i);
M5.Lcd.print("Move servo -> ");
M5.Lcd.println(i);
}

myCobot.setEncoder(i, 1848);
delay(2500);
myCobot.setEncoder(i, 2248);
delay(3000);
myCobot.setEncoder(i, 2048);
delay(2500);
}
Calibration::info();
delay(2000);
}
else{
if(lan == 2){
M5.Lcd.drawString("请先设定关节零位", 20, 20, 1);
}
if(lan == 1){
M5.Lcd.setCursor(0, 30);
M5.Lcd.print("Only move after all servo calibrated");
}
delay(2000);
Calibration::info();
return;
}
}

void Calibration::reset(MycobotBasic &myCobot)
{
M5.Lcd.clear(BLACK);
delay(50);
if(lan == 2){
M5.Lcd.drawString("重新设置", 20, 20, 1);
}
if(lan == 1){
M5.Lcd.setCursor(0, 30);
M5.Lcd.print("Restart to calibrate");
}
calibrate_servo_no = 0;
//关闭扭力输出
myCobot.setFreeMove();
delay(1000);
Calibration::info();
}
20 changes: 20 additions & 0 deletions Arduino/MycobotBasic/examples/miniRoboFlow/Calibration.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
#ifndef Calibration_h
#define Calibration_h

#include <MycobotBasic.h>
#include "config.h"



class Calibration
{
private:
void info();
void init(MycobotBasic &myCobot);
void test(MycobotBasic &myCobot);
void reset(MycobotBasic &myCobot);
public:
void bration(MycobotBasic &myCobot);
};

#endif
101 changes: 101 additions & 0 deletions Arduino/MycobotBasic/examples/miniRoboFlow/Information.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,101 @@
#include "Information.h"

SMSBL sm;

void Connect::info(){
M5.Lcd.clear(BLACK);
M5.Lcd.setTextColor(BLACK);
M5.Lcd.setTextColor(RED);
M5.Lcd.setTextSize(3);
M5.Lcd.setCursor(0, 10);
M5.Lcd.printf("myCobot");
M5.Lcd.setCursor(0, 40);
M5.Lcd.drawFastHLine(0,70,320,GREY);
M5.Lcd.setTextSize(3);
M5.Lcd.println("Status Inspection");
M5.Lcd.setCursor(0, 100);
M5.Lcd.setTextColor(WHITE);
M5.Lcd.setTextSize(2);
M5.Lcd.println("Press A - Servos motor");
M5.Lcd.println();
M5.Lcd.println("Press B - Firmware");
M5.Lcd.println();
M5.Lcd.println("Press C - Exit(1S)");
}

void Connect::test(MycobotBasic &myCobot){
info();
sm.pSerial = &Serial2;
while (1)
{
// put your main code here, to run repeatedly:
M5.update(); // need to call update()
M5.Lcd.setCursor(0,0);
// M5.Lcd.clear(BLACK);

if (M5.BtnA.wasReleased()) {
Connect::testServo(myCobot);
}
if (M5.BtnB.wasReleased()) {
Connect::ReadConfig();
}
if (M5.BtnC.wasReleasefor(1000)) {
break;
}
}
}

void Connect::testServo(MycobotBasic &myCobot){
M5.Lcd.clear(BLACK);
delay(50);
M5.Lcd.setTextSize(3);
M5.Lcd.setCursor(0, 10);
M5.Lcd.setTextColor(WHITE);
M5.Lcd.println("connect test");
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(0, 40);
int state = myCobot.isPoweredOn();
M5.Lcd.print("atom - ");
if(state == 1){
M5.Lcd.setTextColor(GREEN);
M5.Lcd.println("ok");
}else{
M5.Lcd.setTextColor(RED);
M5.Lcd.println("no");
}
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(0, 60);
M5.Lcd.setTextColor(WHITE);
for(int i = 1; i<7;i++){
M5.Lcd.setTextColor(WHITE);
M5.Lcd.print("servo ");
M5.Lcd.print(i);
M5.Lcd.print(" - ");
if(sm.FeedBack(i) != -1){
M5.Lcd.setTextColor(GREEN);
M5.Lcd.println("ok");
}else{
M5.Lcd.setTextColor(RED);
M5.Lcd.println("no");
}
delay(50);
}
M5.update();
delay(3000);
info();
}


void Connect::ReadConfig(){
M5.Lcd.clear(BLACK);
delay(50);
M5.Lcd.setTextSize(3);
M5.Lcd.setCursor(0, 10);
M5.Lcd.println("connect test");
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(0, 40);
M5.Lcd.println("Waiting for development");
M5.update();
delay(3000);
info();
}
19 changes: 19 additions & 0 deletions Arduino/MycobotBasic/examples/miniRoboFlow/Information.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
#ifndef Information_h
#define Information_h

#include "MycobotBasic.h"
#include "SCServo.h"
#include "config.h"

class Connect
{
private:
/* data */
void info();
void testServo(MycobotBasic &myCobot);
void ReadConfig();
public:
void test(MycobotBasic &myCobot);
};

#endif
Loading

0 comments on commit aae1ebe

Please sign in to comment.