微控制类(M5)

按键控制

打开/关闭扭矩，请将下面的代码在arduino中编译并烧录进basic即可

#include <MyCobotBasic.h>
#include <ParameterList.h>

MyCobotBasic myCobot;

//定义一个Angles类型的变量angles，内容如下
Angles angles = { -0.0, -0.0, -0.0, -0.0, -0.0, -0.0 };           


void setup() {

  //打开通讯串口
  myCobot.setup();

  //赋值angles为{ 0, 0, 0, 0, 0, 0 };
  for(auto &val : angles)       
    val = 0.0;

  //关节回到原点
  myCobot.writeAngles(angles, 50);
  delay(5000);
}

void loop() {
  // put your main code here, to run repeatedly:
  M5.update();

  //按键A打开力矩
  if(M5.BtnA.wasPressed()){
     myCobot.powerOn();
     BtnAPress();
  }

  //按键C关闭力矩
  if(M5.BtnC.wasPressed()){
     myCobot.releaseAllservo();
     BtnAPress();
  }
  //按键A
  void BtnAPress(){
  M5.Lcd.clear(BLACK);
  delay(50);
  M5.Lcd.drawString("打开扭矩 ", 20, 20, 1);
  }
  //按键C
  void BtnCPress(){
  M5.Lcd.clear(BLACK);
  delay(50);
  M5.Lcd.drawString("关闭扭矩 ", 20, 20, 1);
  }
}