#include "M5StickC.

h"
#include "bugC.h"

void setup()
{
M5.begin();
Wire.begin(0, 26, 400000);
M5.Lcd.setTextColor(TFT_BLUE);
M5.Lcd.setRotation(1);
M5.Lcd.drawCentreString("Motion Test", 80, 30, 4);
// if add battery, need increase charge current
M5.Axp.SetChargeCurrent(CURRENT_360MA);
}

void loop()
{
// M5.update();
BugCSetColor(0x100000, 0x100000);
BugCSetAllSpeed(100, -100, 100, -100);
delay(1000));
BugCSetAllSpeed(0, 0, 0,0);
delay(30);
BugCSetColor(0x001000, 0x100000);
BugCSetAllSpeed(-100, -100, -100, -100);
delay(100));
BugCSetAllSpeed(0, 0, 0,0);
delay(30);
BugCSetColor(0x001000, 0x001000);
BugCSetAllSpeed(-100, 100, -100, 100);
delay(600);
BugCSetAllSpeed(0, 0, 0,0);
delay(30);
BugCSetColor(0x001000, 0x001000);
BugCSetAllSpeed(100, 100, 100, 100);
delay(100);
BugCSetAllSpeed(0, 0, 0,0);
delay(30);
}