From aae1ebefeaad1c9f6779430999a345c4f993cbfa Mon Sep 17 00:00:00 2001 From: wxy6655 Date: Fri, 30 Jul 2021 10:34:15 +0800 Subject: [PATCH] update new firmware MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit can use maincontrol and calibration ，transponder，information --- .../examples/miniRoboFlow/Calibration.cpp | 168 +++++ .../examples/miniRoboFlow/Calibration.h | 20 + .../examples/miniRoboFlow/Information.cpp | 101 +++ .../examples/miniRoboFlow/Information.h | 19 + .../examples/miniRoboFlow/MainControl.cpp | 682 ++++++++++++++++++ .../examples/miniRoboFlow/MainControl.h | 24 + .../examples/miniRoboFlow/TransPonder.cpp | 212 ++++++ .../examples/miniRoboFlow/config.h | 16 + .../examples/miniRoboFlow/miniRoboFlow.ino | 131 ++++ .../examples/miniRoboFlow/transponder.h | 27 + 10 files changed, 1400 insertions(+) create mode 100644 Arduino/MycobotBasic/examples/miniRoboFlow/Calibration.cpp create mode 100644 Arduino/MycobotBasic/examples/miniRoboFlow/Calibration.h create mode 100644 Arduino/MycobotBasic/examples/miniRoboFlow/Information.cpp create mode 100644 Arduino/MycobotBasic/examples/miniRoboFlow/Information.h create mode 100644 Arduino/MycobotBasic/examples/miniRoboFlow/MainControl.cpp create mode 100644 Arduino/MycobotBasic/examples/miniRoboFlow/MainControl.h create mode 100644 Arduino/MycobotBasic/examples/miniRoboFlow/TransPonder.cpp create mode 100644 Arduino/MycobotBasic/examples/miniRoboFlow/config.h create mode 100644 Arduino/MycobotBasic/examples/miniRoboFlow/miniRoboFlow.ino create mode 100644 Arduino/MycobotBasic/examples/miniRoboFlow/transponder.h diff --git a/Arduino/MycobotBasic/examples/miniRoboFlow/Calibration.cpp b/Arduino/MycobotBasic/examples/miniRoboFlow/Calibration.cpp new file mode 100644 index 0000000..81852c1 --- /dev/null +++ b/Arduino/MycobotBasic/examples/miniRoboFlow/Calibration.cpp @@ -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(); +} diff --git a/Arduino/MycobotBasic/examples/miniRoboFlow/Calibration.h b/Arduino/MycobotBasic/examples/miniRoboFlow/Calibration.h new file mode 100644 index 0000000..93b5985 --- /dev/null +++ b/Arduino/MycobotBasic/examples/miniRoboFlow/Calibration.h @@ -0,0 +1,20 @@ +#ifndef Calibration_h +#define Calibration_h + +#include +#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 \ No newline at end of file diff --git a/Arduino/MycobotBasic/examples/miniRoboFlow/Information.cpp b/Arduino/MycobotBasic/examples/miniRoboFlow/Information.cpp new file mode 100644 index 0000000..3bdc86c --- /dev/null +++ b/Arduino/MycobotBasic/examples/miniRoboFlow/Information.cpp @@ -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(); +} diff --git a/Arduino/MycobotBasic/examples/miniRoboFlow/Information.h b/Arduino/MycobotBasic/examples/miniRoboFlow/Information.h new file mode 100644 index 0000000..3f5865d --- /dev/null +++ b/Arduino/MycobotBasic/examples/miniRoboFlow/Information.h @@ -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 diff --git a/Arduino/MycobotBasic/examples/miniRoboFlow/MainControl.cpp b/Arduino/MycobotBasic/examples/miniRoboFlow/MainControl.cpp new file mode 100644 index 0000000..0499847 --- /dev/null +++ b/Arduino/MycobotBasic/examples/miniRoboFlow/MainControl.cpp @@ -0,0 +1,682 @@ +#include "MainControl.h" + + +typedef struct{ + int joint_angle[6]; +} joint_angles_enc; + +int data_len_max = 1000; +Angles jae[1000]; + +int girrep_data[1000]; + +byte control_pin = 26; // PIN 26 high -> loop play data from Flash + +int rec_data_len = 0; +byte mycobot_mode = 0; + +int error_display_time = 3000; +int EXIT = false; + + +void MainControl::Control(MycobotBasic &myCobot){ + pinMode(control_pin, INPUT); + myCobot.setLEDRGB(0,255,0); + MainControl::updateMode(myCobot, mycobot_mode); + MainControl::displayInfo(myCobot, 0); + EXIT = false; + while(1){ + M5.update(); + byte btn_pressed = 0; + if (M5.BtnA.wasPressed()) { + btn_pressed = 1; + MainControl::updateMode(myCobot, btn_pressed); + } + if(M5.BtnB.wasPressed()){ + btn_pressed = 2; + MainControl::updateMode(myCobot, btn_pressed); + } + if (M5.BtnC.wasPressed()) { + btn_pressed = 3; + MainControl::updateMode(myCobot, btn_pressed); + } + if(EXIT){break;} + MainControl::IO(myCobot); + } +} + +void MainControl::updateMode(MycobotBasic &myCobot, byte btn_pressed) +{ + if (mycobot_mode == 0) + { + switch (btn_pressed) + { + case 1: + mycobot_mode = 1; + break; + case 2: + mycobot_mode = 2; + break; + case 3: + myCobot.setFreeMove(); + mycobot_mode = 0; + EXIT = true; + break; + } + MainControl::displayInfo(myCobot, mycobot_mode); + } + else if(mycobot_mode == 1) + { + switch (btn_pressed) + { + case 1: + mycobot_mode = 11; + MainControl::displayInfo(myCobot, mycobot_mode); + MainControl::play(myCobot); // play loop from ram + + break; + case 2: + mycobot_mode = 12; + MainControl::playFromFlash(myCobot); // play loop from flash + + break; + case 3: + mycobot_mode = 0; // get back + MainControl::displayInfo(myCobot, mycobot_mode); + break; + } + + } + + else if(mycobot_mode == 2) + { + + switch (btn_pressed) + { + case 1: + mycobot_mode = 21; + MainControl::displayInfo(myCobot, mycobot_mode); + MainControl::record(myCobot); + + // finish record + MainControl::displayInfo(myCobot, 33); + delay(2000); + + // recover to original + mycobot_mode = 0; + MainControl::displayInfo(myCobot, mycobot_mode); + break; + + case 2: + mycobot_mode = 22; // record into ram as well + MainControl::displayInfo(myCobot, mycobot_mode); + MainControl::recordIntoFlash(myCobot); + break; + case 3: + mycobot_mode = 0; + MainControl::displayInfo(myCobot, mycobot_mode); + break; + } + + } + + else if((mycobot_mode == 11)||(mycobot_mode == 12)) + { + switch (btn_pressed) + { + case 1: + Serial.println("Continue Play"); + + break; + case 2: + Serial.println("Pause"); + + break; + case 3: + Serial.println("Stop"); + mycobot_mode = 0; + break; + } + } + + else if((mycobot_mode == 21)||(mycobot_mode == 22)) + { + switch (btn_pressed) + { + case 2: + Serial.println("Save and Stop"); + break; + case 3: + Serial.println("stop record"); + break; + + } + + mycobot_mode = 0; + MainControl::displayInfo(myCobot, mycobot_mode); + } + +} + +void MainControl::displayInfo(MycobotBasic &myCobot, byte mc_mode) +{ + M5.Lcd.clear(BLACK); + delay(50); + M5.Lcd.setTextSize(2); + int buttom_y = 190; + int buttom_1y = 210; + int buttom_2y = 210; + + switch (mc_mode) { + case 0: + { + M5.Lcd.fillScreen(0); + if (lan == 2) + { + M5.Lcd.drawString(" MyCobot-拖动示教", 20, 40, 1); + M5.Lcd.drawString("播放", 60, buttom_y, 1); + M5.Lcd.drawString("录制", 160, buttom_y, 1); + M5.Lcd.drawString("退出", 260, buttom_y, 1); + M5.update(); + } + else if (lan == 1) + { + 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("MainControl Menu"); + M5.Lcd.setTextSize(2); + M5.Lcd.setTextColor(WHITE); + M5.Lcd.setCursor(30, 210); + M5.Lcd.print("Play"); + M5.Lcd.setCursor(120, 210); + M5.Lcd.print("Record"); + M5.Lcd.setCursor(230, 210); + M5.Lcd.print("Exit"); + M5.update(); + } + break; + } + + case 1: + { + M5.Lcd.fillScreen(0); + if (lan == 2) + { + M5.Lcd.drawString(" 请选择示教路径的播放位置", 10, 40, 1); + M5.Lcd.drawString("缓存", 60, buttom_y, 1); + M5.Lcd.drawString("储存卡", 160, buttom_y, 1); + M5.Lcd.drawString("返回", 260, buttom_y, 1); + } + else if (lan == 1) + { + M5.Lcd.setCursor(20, 40); + M5.Lcd.print("Playing for Ram/Flash?"); + M5.Lcd.setCursor(40, buttom_1y); + M5.Lcd.print("Ram"); + M5.Lcd.setCursor(136, buttom_1y); + M5.Lcd.print("Flash"); + M5.Lcd.setCursor(240, buttom_1y); + M5.Lcd.print("Back"); + } + break; + } + case 2: + { + M5.Lcd.fillScreen(0); + if (lan == 2) + { + M5.Lcd.drawString(" 请选择示教视频储存路径", 20, 40, 1); + M5.Lcd.drawString("缓存", 60, buttom_y, 1); + M5.Lcd.drawString("储存卡", 160, buttom_y, 1); + M5.Lcd.drawString("返回", 260, buttom_y, 1); + } + else if (lan == 1) + { + M5.Lcd.setCursor(20, 40); + M5.Lcd.print("Recording to Ram/Flash?"); + M5.Lcd.setCursor(40, buttom_1y); + M5.Lcd.print("Ram"); + M5.Lcd.setCursor(136, buttom_1y); + M5.Lcd.print("Flash"); + M5.Lcd.setCursor(240, buttom_1y); + M5.Lcd.print("Back"); + } + break; + } + case 11: + { + M5.Lcd.fillScreen(0); + if (lan == 2) + { + M5.Lcd.drawString(" 正在执行缓存中的路径", 20, 40, 1); + M5.Lcd.drawString(" 播放中...", 20, 70, 1); + M5.Lcd.drawString("开始", 60, buttom_y, 1); + M5.Lcd.drawString("暂停", 160, buttom_y, 1); + M5.Lcd.drawString("结束", 260, buttom_y, 1); + } + else if (lan == 1) + { + M5.Lcd.setCursor(20, 40); + M5.Lcd.print("Play from Ram\n Playing..."); + M5.Lcd.setCursor(40, buttom_2y); + M5.Lcd.print("Play"); + M5.Lcd.setCursor(130, buttom_2y); + M5.Lcd.print("Pause"); + M5.Lcd.setCursor(230, buttom_2y); + M5.Lcd.print("Stop"); + } + break; + } + case 12: + { + M5.Lcd.fillScreen(0); + if (lan == 2) + { + M5.Lcd.drawString(" 正在执行储存卡中的路径", 20, 40, 1); + M5.Lcd.drawString(" 播放中...", 20, 70, 1); + M5.Lcd.drawString("开始", 60, buttom_y, 1); + M5.Lcd.drawString("暂停", 160, buttom_y, 1); + M5.Lcd.drawString("结束", 260, buttom_y, 1); + } + else if (lan == 1) + { + M5.Lcd.setCursor(0, 40); + M5.Lcd.print(" Play from Flash/nPlaying"); + M5.Lcd.setCursor(40, buttom_2y); + M5.Lcd.print("Play"); + M5.Lcd.setCursor(130, buttom_2y); + M5.Lcd.print("Pause"); + M5.Lcd.setCursor(230, buttom_2y); + M5.Lcd.print("Stop"); + } + break; + } + + case 21: + { + M5.Lcd.fillScreen(0); + if (lan == 2) + { + M5.Lcd.drawString(" 录制并存入缓存", 20, 40, 1); + M5.Lcd.drawString(" 录制中...", 20, 70, 1); + M5.Lcd.drawString(" 停止录制并保存", 5, buttom_y, 1); + } + else if (lan == 1) + { + M5.Lcd.setCursor(0, 40); + M5.Lcd.print("Record into Ram\nRecording..."); + M5.Lcd.setCursor(5, buttom_1y); + M5.Lcd.print("Stop Recording and Save"); + } + break; + } + case 22: + { + M5.Lcd.fillScreen(0); + if (lan == 2) + { + M5.Lcd.drawString(" 录制并存入储存卡", 20, 40, 1); + M5.Lcd.drawString(" 录制中...", 20, 70, 1); + M5.Lcd.drawString(" 停止录制并保存", 5, buttom_y, 1); + } + if (lan == 1) + { + M5.Lcd.setCursor(0, 40); + M5.Lcd.print("Record into Flash\nRecording..."); + M5.Lcd.setCursor(5, buttom_y); + M5.Lcd.print("Stop Recording and Save"); + } + + break; + } + + case 32: // Stop recording + { + M5.Lcd.fillScreen(0); + if(lan == 2){ + M5.Lcd.drawString(" 暂停中", 20, 40, 1); + M5.Lcd.drawString("开始", 60, buttom_y, 1); + M5.Lcd.drawString("暂停", 160, buttom_y, 1); + M5.Lcd.drawString("结束", 260, buttom_y, 1); + } + else if(lan == 1){ + M5.Lcd.setCursor(0,40); + M5.Lcd.print(" Puase Now"); + M5.Lcd.setCursor(40,buttom_2y); + M5.Lcd.print("Play"); + M5.Lcd.setCursor(130,buttom_2y); + M5.Lcd.print("Pause"); + M5.Lcd.setCursor(230,buttom_2y); + M5.Lcd.print("Stop"); + } + break; + } + + case 33: // Stop recording + { + M5.Lcd.fillScreen(0); + if(lan == 2){ + M5.Lcd.drawString(" 保存录制", 20, 40, 1); + } + else if(lan == 1){ + M5.Lcd.setCursor(0,40); + M5.Lcd.print(" Saved Recording"); + } + break; + } + + case 41: // Play from Flash + { + M5.Lcd.fillScreen(0); + if(lan == 2){ + M5.Lcd.drawString("从储存卡中获取数据", 20, 40, 1); + } + else if(lan == 1){ + M5.Lcd.setCursor(0,40); + M5.Lcd.print("Getting data from Flash"); + } + break; + } + + case 42: // Record from Flash + { + M5.Lcd.fillScreen(0); + if(lan == 2){ + M5.Lcd.drawString("将数据保存到储存卡", 20, 40, 1); + } + else if(lan == 1){ + M5.Lcd.setCursor(0,40); + M5.Lcd.print("Saving Data into Flash"); + } + break; + } + + case 51: // loop play from sram + { + M5.Lcd.fillScreen(0); + if(lan == 2){ + M5.Lcd.drawString("IO口状态", 20, 40, 1); + } + else if(lan == 1){ + M5.Lcd.setCursor(0,40); + M5.Lcd.print("IO Pin Active!"); + } + break; + } + + case 52: // loop play from sram + { + M5.Lcd.fillScreen(0); + if(lan == 2){ + M5.Lcd.drawString("数据不足，无法播放", 20, 40, 1); + } + else if(lan == 1){ + M5.Lcd.setCursor(0,40); + M5.Lcd.print("Data too short, not playing"); + } + break; + } + + case 53: // loop play from sram + { + M5.Lcd.fillScreen(0); + if(lan == 2){ + M5.Lcd.drawString("无数据", 20, 40, 1); + } + else if(lan == 1){ + M5.Lcd.setCursor(0,40); + M5.Lcd.print("Empty data!"); + } + break; + } + default: ;break; + } +} + +void MainControl::record(MycobotBasic &myCobot) // is stop +{ + myCobot.setLEDRGB(255,255,0); + // record mode : 1- record to ram; 2- record to flash + rec_data_len = 0; + Angles _data; + int _encoder = myCobot.getEncoder(7); + delay(35); + if (_encoder > 0) + { + for (int data_index = 0; data_index data_len_max) break; + } + + // update the len + rec_data_len = index - 1; + + // play from flash + M5.update(); + MainControl::displayInfo(myCobot, mycobot_mode); + MainControl::play(myCobot); +} + +void MainControl::recordIntoFlash(MycobotBasic &myCobot) +{ + // recording data + MainControl::record(myCobot); + M5.update(); + // show saving to recording + MainControl::displayInfo(myCobot, 33); + + // initialize flash + if(!SPIFFS.begin(FORMAT_SPIFFS_IF_FAILED)){ + Serial.println("SPIFFS Mount Failed"); + return; + } + + // list exisiting files + myCobot.saver.listDir(SPIFFS, "/", 0); + + // clean exsiting file + myCobot.saver.writeFile(SPIFFS, FILENAME, " "); + + // check time + File file = SPIFFS.open(FILENAME, FILE_APPEND); + + for (int data_index = 0; data_index < rec_data_len; data_index ++) + { + String data_output = ""; + for (int l = 0; l <6 ; l ++) + { + data_output += jae[data_index][l]; + data_output += ","; + } + data_output += girrep_data[data_index]; + data_output += "\n"; + file.print(data_output); + } + + // recover to original + mycobot_mode = 0; + MainControl::displayInfo(myCobot, mycobot_mode); +} + +void MainControl::IO(MycobotBasic &myCobot) +{ + int pin_data = digitalRead(control_pin); + + return; + if (pin_data == 1) + { + MainControl::displayInfo(myCobot, 51); + delay(error_display_time); + + mycobot_mode = 12; + MainControl::play(myCobot); + } + +} + + + +/* +bool MainControl::checkDataLen(MycobotBasic &myCobot) +{ + + if (rec_data_len == 0){ + MainControl::displayInfo(myCobot, 53); + delay(error_display_time); + return 0; + } + + if (rec_data_len < 10){ + MainControl::displayInfo(myCobot, 52); + delay(error_display_time); + return 0; + } + return 1; +} +*/ diff --git a/Arduino/MycobotBasic/examples/miniRoboFlow/MainControl.h b/Arduino/MycobotBasic/examples/miniRoboFlow/MainControl.h new file mode 100644 index 0000000..7a32bb1 --- /dev/null +++ b/Arduino/MycobotBasic/examples/miniRoboFlow/MainControl.h @@ -0,0 +1,24 @@ +#ifndef MainControl_h +#define MainControl_h + +#include +#include "config.h" + + + +class MainControl +{ +private: + void updateMode(MycobotBasic &myCobot, byte btn_pressed); + void displayInfo(MycobotBasic &myCobot, byte mc_mode); + void record(MycobotBasic &myCobot); // is stop; + void play(MycobotBasic &myCobot); // is stop is pause + void playFromFlash(MycobotBasic &myCobot); + void recordIntoFlash(MycobotBasic &myCobot); + void IO(MycobotBasic &myCobot); + // bool checkDataLen(); +public: + void Control(MycobotBasic &myCobot); +}; + +#endif \ No newline at end of file diff --git a/Arduino/MycobotBasic/examples/miniRoboFlow/TransPonder.cpp b/Arduino/MycobotBasic/examples/miniRoboFlow/TransPonder.cpp new file mode 100644 index 0000000..e650492 --- /dev/null +++ b/Arduino/MycobotBasic/examples/miniRoboFlow/TransPonder.cpp @@ -0,0 +1,212 @@ +#include "transponder.h" + +// MycobotBasic myCobot; + +// #define +typedef unsigned char u8; + +void Transponder::init() { + // myCobot.setup(); + // myCobot.powerOn(); + pinMode(15, OUTPUT); // 1 + pinMode(5, OUTPUT); // 2 + pinMode(35, INPUT); // 2 + pinMode(36, INPUT); // 2 + delay(100); + digitalWrite(15, 1); // 1 + digitalWrite(5, 1); // 2 + info(); +} + +void Transponder::ponder(MycobotBasic &myCobot) { + init(); + EXIT = false; + while (!EXIT) + { + readData(myCobot); + } + + +} + +bool Transponder::checkHeader(MycobotBasic &myCobot) +{ + u8 bDat; + u8 bBuf[2] = { 0, 0 }; + u8 Cnt = 0; + while (true) + { + if (!readSerial(myCobot, &bDat, 1)) + return 0; + bBuf[1] = bBuf[0]; + bBuf[0] = bDat; + if (bBuf[0] == HEADER && bBuf[1] == HEADER){ + break; + } + ++Cnt; + if (Cnt > 64) + return 0; + } + return 1; +} + +int Transponder::readSerial(MycobotBasic &myCobot, unsigned char* nDat, int nLen) +{ + + int Size = 0; + int rec_data; + unsigned long t_begin = millis(); + unsigned long t_use; + + while (true) + { + M5.update(); + if (M5.BtnA.wasReleased() || M5.BtnA.pressedFor(1000, 200)) { + connect_ATOM(myCobot); + } + if(M5.BtnC.wasReleasefor(1000)) { EXIT = true; break;} + + if(Serial.available()>0) + { + rec_data = Serial.read(); // readSerial + Serial2.write(rec_data); + + if (rec_data != IORecWrong) + { + if (nDat) + nDat[Size] = rec_data; + ++Size; + t_begin = millis(); + } + + if (Size >= nLen) + break; + t_use = millis() - t_begin; + + if (t_use > IOTimeOut_1) + break; + } + // read serial 2 + if (Serial2.available() > 0) { // If anything comes in Serial 2 + Serial.write(Serial2.read()); // read it and send it out Serial (USB) + } + } + return Size; +} + +void Transponder::rFlushSerial() +{ + while (Serial.read() != -1) + ; +} + +int Transponder::readData(MycobotBasic &myCobot) +{ + rFlushSerial(); + if (!Transponder::checkHeader(myCobot)) + return -1; + + u8 data_len[1]; + u8 r_data_4[4]; + u8 r_data_3[3]; + if (Transponder::readSerial(myCobot, data_len, 1) != 1) + return -1; + switch (static_cast(data_len[0])) + { + case 4: + Transponder::readSerial(myCobot, r_data_4, 4); + switch (int(r_data_4[0])) + { + case 0xa0: + { + //0xfe 0xfe 0x04 0xa0 pin_no pin_data 0xfa + byte pin_no = r_data_4[1]; + pinMode(pin_no, OUTPUT); + delay(5); + bool pin_data = r_data_4[2]; + digitalWrite(pin_no,pin_data); + Serial.write(0xfe); + Serial.write(0xfe); + Serial.write(0x04); + Serial.write(0xa0); + Serial.write(pin_no); + Serial.write(pin_data); + Serial.write(0xfa); + } + break; + } + break; + case 3: + Transponder::readSerial(myCobot, r_data_3, 3); + switch (int(r_data_3[0])) + { + case 0xa1: + { + byte pin_no = r_data_3[1]; + pinMode(pin_no, INPUT); + delay(5); + bool pin_state = digitalRead(pin_no); + delay(5); + Serial.write(0xfe); + Serial.write(0xfe); + Serial.write(0x04); + Serial.write(0xa1); + Serial.write(pin_no); + Serial.write(pin_state); + Serial.write(0xfa); + } + break; + } + break; + + default: + break; + } + + while(Serial.available()>0) {Serial2.write(Serial.read()); } + while(Serial2.available()>0) {Serial.write(Serial2.read());} + +} + +void Transponder::connect_ATOM(MycobotBasic &myCobot){ + M5.Lcd.clear(BLACK); + delay(50); + M5.Lcd.setTextColor(YELLOW); + M5.Lcd.setTextSize(3); + M5.Lcd.setCursor(55, 5); + M5.Lcd.println("Connect test"); + M5.Lcd.setCursor(5, 40); + int state = myCobot.isPoweredOn(); + if(state == 1){ + M5.Lcd.println("Atom: ok"); + }else{ + M5.Lcd.println("Atom: no"); + } + M5.update(); + delay(10); + delay(2000); + info(); +} + +void Transponder::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 Transponder"); + M5.Lcd.setTextSize(2); + M5.Lcd.setTextColor(WHITE); + M5.Lcd.setCursor(0, 100); + M5.Lcd.println("Press A - Atom ConnectTest"); + M5.Lcd.println(); + M5.Lcd.println("Press C - Exit(1S)"); + M5.Lcd.setCursor(40, 210); + M5.update(); + delay(10); +} diff --git a/Arduino/MycobotBasic/examples/miniRoboFlow/config.h b/Arduino/MycobotBasic/examples/miniRoboFlow/config.h new file mode 100644 index 0000000..24ce59a --- /dev/null +++ b/Arduino/MycobotBasic/examples/miniRoboFlow/config.h @@ -0,0 +1,16 @@ +#ifndef Config_h +#define Config_h + +#include + +#define lan 1 + +#define GREY 0x5AEB + +#define state_addr 0X0f + +#define App_number 3 + +#define HEADER 0XFE + +#endif diff --git a/Arduino/MycobotBasic/examples/miniRoboFlow/miniRoboFlow.ino b/Arduino/MycobotBasic/examples/miniRoboFlow/miniRoboFlow.ino new file mode 100644 index 0000000..0e9a868 --- /dev/null +++ b/Arduino/MycobotBasic/examples/miniRoboFlow/miniRoboFlow.ino @@ -0,0 +1,131 @@ +#include +#include "transponder.h" +#include "MainControl.h" +#include "Calibration.h" +#include "Information.h" +#include "config.h" + +MycobotBasic myCobot; +Transponder tp; +MainControl mc; +Calibration cb; +Connect ct; + +int state = 0; +// bool EXIT = false; +int y_pos[] = {70, 95, 120, 145}; + + +void setup(){ + myCobot.setup(); + myCobot.powerOn(); //启动机械臂 + delay(50); + EEPROM.begin(EEPROM_SIZE); //EEPROM_SIZE=64 + + if (EEPROM.read(state_addr)) + { + state = EEPROM.read(state_addr) - 1; //写数据 + program_selection(myCobot, state); + }else{ + EEPROM.write(state_addr, 0); //写数据 + } + EEPROM.commit(); //保存更改的数据 + M5.Lcd.clear(BLACK); + menu_choice(); + select(); +} + +void loop(){ + M5.update(); + if (M5.BtnA.wasPressed()) { + state += 1; + select(); + } + if (M5.BtnB.wasPressed()) { + state -= 1; + select(); + } + if (M5.BtnC.wasPressed()) { + EEPROM.begin(EEPROM_SIZE); //申请操作到地址4095,size=目标地址+1 + EEPROM.write(state_addr, state + 1); //写数据 + EEPROM.commit(); //保存更改的数据 + program_selection(myCobot, state); + M5.Lcd.clear(BLACK); + menu_choice(); + select(); + } +} + +void program_selection(MycobotBasic &myCobot, int state){ + switch (state) + { + case 0: + mc.Control(myCobot); + break; + case 1: + cb.bration(myCobot); + break; + case 2: + tp.ponder(myCobot); + break; + case 3: + ct.test(myCobot); + break; + default: + break; + } + menu_choice(); +} + +void menu_choice(){ + + M5.Lcd.setTextSize(3); + M5.Lcd.setTextColor(RED); + M5.Lcd.setCursor(20,10); + M5.Lcd.print("miniRoboFlow"); + + M5.Lcd.setTextSize(2); + M5.Lcd.setCursor(250,30); + M5.Lcd.setTextColor(GREY); + M5.Lcd.print("v1.0"); + + M5.Lcd.drawFastHLine(0,45,320,GREY); + M5.Lcd.drawFastHLine(0,195,320,GREY); + + M5.Lcd.setTextSize(2); + M5.Lcd.setCursor(30,y_pos[0]); + M5.Lcd.setTextColor(WHITE); + M5.Lcd.print("MainControl"); + M5.Lcd.setCursor(30,y_pos[1]); + M5.Lcd.setTextColor(WHITE); + M5.Lcd.print("Calibration"); + M5.Lcd.setCursor(30,y_pos[2]); + M5.Lcd.setTextColor(WHITE); + M5.Lcd.print("Transponder"); + M5.Lcd.setCursor(30,y_pos[3]); + M5.Lcd.setTextColor(WHITE); + M5.Lcd.print("Information"); + + M5.Lcd.setCursor(35, 210); + M5.Lcd.print("DOWN"); + M5.Lcd.setCursor(140, 210); + M5.Lcd.print(" UP "); + M5.Lcd.setCursor(235, 210); + M5.Lcd.print(" OK "); + +} + +void select(){ + if (state < 0) + { + state = App_number; + }if (state > App_number) + { + state = 0; + } + M5.Lcd.fillRect(0, 70, 26, 90, BLACK); + M5.Lcd.setTextSize(2); + M5.Lcd.setCursor(0,y_pos[state]); + M5.Lcd.setTextColor(WHITE); + M5.Lcd.print(" >"); +} diff --git a/Arduino/MycobotBasic/examples/miniRoboFlow/transponder.h b/Arduino/MycobotBasic/examples/miniRoboFlow/transponder.h new file mode 100644 index 0000000..7941d38 --- /dev/null +++ b/Arduino/MycobotBasic/examples/miniRoboFlow/transponder.h @@ -0,0 +1,27 @@ +#ifndef transponder_h +#define transponder_h + +#include +#include "config.h" + + + +class Transponder +{ +private: + bool EXIT = false; + bool checkHeader(MycobotBasic &myCobot); + int readSerial(MycobotBasic &myCobot, unsigned char* nDat, int nLen); + void rFlushSerial(); + int readData(MycobotBasic &myCobot); + void connect_ATOM(MycobotBasic &myCobot); + void info(); +public: + void ponder(MycobotBasic &myCobot); + void init(); + +}; + +#endif + +