01: // SainSmart Instabots Upright Rover rev. 2.0

02: // https://www.sainsmart.com/products/instabots-self-balancing-robot-v2

03: #include "M5Atom.h"

04: //M5Stamp_Pico用が未だリリースされてないので

05: //同じPico系のM5Atomライブラリーを使用しています。

06: #define BLYNK_PRINT Serial          // BLYNK

07: #define BLYNK_USE_DIRECT_CONNECT    // BLYNK

08: #include "BlynkSimpleEsp32_BLE.h"   // BLYNK

09: #include "I2Cdev.h"  // MPU6050-0.2.1

10: #include "MPU6050.h" // MPU6050-0.2.1

11: #include "Wire.h"

12: char auth[] = "**************************"; // ***Blynkから取得したauth codeを記入

13:

14: MPU6050 accelgyro;

15: MPU6050 initialize;

16: int16_t ax, ay, az;

17: int16_t gx, gy, gz;

18: #define Gry_offset 0  //The offset of the gyro

19: #define Gyr_Gain 16.348

20: #define Angle_offset 0 // 倒立状態で前後どちらかへ進む場合は要調整

21: #define pi 3.14159

22: #define min(X, Y) (((X)<(Y))?(X):(Y))

23:

24: float kp, ki, kd;

25: float Angle_Raw, Angle_Filtered, omega;

26: float Turn_Speed = 0;

27: float Run_Speed = 0, Run_Speed_K = 0;

28: float LOutput, ROutput;

29: unsigned long preTime = 0;

30: unsigned long lastTime;

31: float Input, Output;

32: float errSum, dErr, error, lastErr;

33: int timeChange;

34: int xVal;

35: int yVal;

36:

37: uint8_t DisBuff[2 + 5 * 5 * 3]; // 内蔵RGBカラーLED

38:

39: void setBuff(uint8_t Rdata, uint8_t Gdata, uint8_t Bdata) { // 内蔵RGBカラーLEDの設定

40:   DisBuff[0] = 0x05;

41:   DisBuff[1] = 0x05;

42:   for (int i = 0; i < 25; i++) {

43:     DisBuff[2 + i * 3 + 0] = Rdata;

44:     DisBuff[2 + i * 3 + 1] = Gdata;

45:     DisBuff[2 + i * 3 + 2] = Bdata;

46:   }

47: }

48:

49: void setup() {

50:   Wire.begin(21，22); // Atom Liteの場合、pin番号が必要

51:   Serial.begin(115200);

52:   M5.begin(true, false, true); // RGBカラーLED

53:   delay(10);

54:   setBuff(0xff, 0x00, 0x00);   // RGBカラーLED

55:   M5.dis.displaybuff(DisBuff); // RGBカラーLED

56:   Blynk.setDeviceName("M5Stamp_BLE"); // Blinkのデバイス名（任意名必須）

57:   Blynk.begin(auth);

58:   accelgyro.initialize();

59:   initialize.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);;

60:   for (int i = 0; i < 200; i++) { // Looping 200 times to get the real gesture when starti61: ng

62:     Filter();

63:   }

64:   if (abs(Angle_Filtered) < 45) { // Start the robot after cleaning data

65:     omega = Angle_Raw = Angle_Filtered = 0;

66:     Filter();

67:     Output = error = errSum = dErr = Run_Speed = Turn_Speed = 0;

68:     PID();

69:   }

70:   ledcSetup(0, 500, 8); // ESP32系のPWM設定、ここでは8ビットを使用

71:   ledcSetup(1, 500, 8);

72:   ledcSetup(2, 500, 8);

73:   ledcSetup(3, 500, 8);

74:  // GPIOの設定:M5 Stamp Picoを使用の場合

75:   ledcAttachPin(26, 0); // GPIOの26pinに設定　モータドライバーIN1へ

76:   ledcAttachPin(18, 1); // GPIOの18pinに設定　モータドライバーIN2へ

77:   ledcAttachPin(19, 2); // GPIOの19pinに設定　モータドライバーIN3へ

78:   ledcAttachPin(25, 3); // GPIOの25pinに設定　モータドライバーIN4へ

79:  // GPIOの設定: M5 Stack Atom liteを使用の場合

80:   ledcAttachPin(19, 0); // GPIOの26pinに設定　モータドライバーIN1へ

81:   ledcAttachPin(23, 1); // GPIOの18pinに設定　モータドライバーIN2へ

82:   ledcAttachPin(25, 2); // GPIOの19pinに設定　モータドライバーIN3へ

83:   ledcAttachPin(33, 3); // GPIOの25pinに設定　モータドライバーIN4へ

84:   ledcWrite(0, 0); // PWM出力の初期設定

85:   ledcWrite(1, 0);

86:   ledcWrite(2, 0);

87:   ledcWrite(3, 0);

88:   yVal = 30; // 初期設定

89:   xVal = 25; // 初期設定

90:   delay(100);

91: }

92:

93: BLYNK_WRITE(V0) { // Blynk V0 トグルボタン（パワーの切り替え）の設定

94:   int button1 = param.asInt();

95:   if (button1 == 1) { // High設定　：High/Lowの値は走路の条件により要調整

96:     yVal = 60;

97:     xVal = 40;

98:   } else {            // Low設定

99:     yVal = 30;

100:     xVal = 25;

101:   }

102: }

103:

104: BLYNK_WRITE(V1) { // Blynk V1 ジョイスティックの受信設定

105:   long Joy_x = param[0].asInt();

106:   long Joy_y = param[1].asInt();

107:

108:   if (Joy_y >= 100) { // 前進

109:     Run_Speed_K = map(Joy_y, 100, 255, 0, yVal);

110:     Run_Speed += Run_Speed_K;

111:     min(80,Run_Speed);

112:     setBuff(0x40, 0x40, 0x40); // LEDを白色に設定

113:   }

114:   else if (Joy_y <= -100) { // 後退

115:     Run_Speed_K = map(Joy_y, -100, -255, 0, -yVal);

116:     Run_Speed += Run_Speed_K;

117:     min(80,Run_Speed);

118:     setBuff(0x40, 0x00, 0x00); // LEDを赤色に設定

119:   }

120:   else {

121:     Run_Speed_K = 0;

122:   }

123:

124:   if (Joy_x >= 100) { // 右旋回

125:     Turn_Speed = map(Joy_x, 100, 255, 0, xVal);

126:     setBuff(0x40, 0x00, 0x40); // LEDをマゼンタ色に設定

127:   }

128:   else if (Joy_x <= -100) { // 左旋回

129:     Turn_Speed = map(Joy_x, -100, -255, 0, -xVal);

130:     setBuff(0x00, 0x00, 0x40); // LEDを青色に設定

131:   }

132:   else {

133:     Turn_Speed = 0;

134:   }

135:

136:   if ((Joy_x == 0) && (Joy_y == 0)) { // 停止

137:     setBuff(0x00, 0x40, 0x00); // LEDを緑色に設定

138:   }

139:   M5.dis.displaybuff(DisBuff);

140: }

141:

142: void loop() {

143:   Blynk.run();  // Blynk ON

144:   Filter();

145:   if (abs(Angle_Filtered) < 45) { // 前後45度以上倒すと一時停止

146:     PID();

147:     MotorControl();

148:   }

149:   else  { // 停止

150:     ledcWrite(0, 0);

151:     ledcWrite(1, 0);

152:     ledcWrite(2, 0);

153:     ledcWrite(3, 0);

154:

155:     for (int i = 0; i < 100; i++) { // Keep reading the gesture after falling down

156:       Filter();

157:     }

158:     if (abs(Angle_Filtered) < 45) { // Empty datas and restart the robot automaticly

159:       for (int i = 0; i <= 500; i++) { // Reset the robot and delay 2 seconds

160:         omega = Angle_Raw = Angle_Filtered = 0;

161:         Filter();

162:         Output = error = errSum = dErr = Run_Speed = Turn_Speed = 0;

163:         PID();

164:       }

165:     }

166:   }

167: }

168:

169: void Filter() { // V2から追加されたフィルター

170:   // Raw datas from MPU6050

171:   accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

172:   Angle_Raw = (atan2(ay, az) * 180 / pi + Angle_offset);

173:   omega =  gx / Gyr_Gain + Gry_offset;

174:   // Filters datas to get the real gesture

175:   unsigned long now = millis();

176:   float dt = (now - preTime) / 1000.00;

177:   preTime = now;

178:   float K = 0.8;

179:   float A = K / (K + dt);

180:   Angle_Filtered = A * (Angle_Filtered + omega * dt) + (1 - A) * Angle_Raw;

181: }

182:

183: void PID() { // PIDの設定は挙動を左右するので最良点を探して調整する

184:   kp = 15;   // 要調整

185:   ki = 0.10; // 要調整

186:   kd = 250;  // 要調整

187:   unsigned long now = millis();

188:   timeChange = (now - lastTime);

189:   lastTime = now;

190:   error = Angle_Filtered;  // Proportion

191:   errSum += error * timeChange;  // Integration

192:   dErr = (error - lastErr) / timeChange;  // Differentiation

193:   Output = kp * error + ki * errSum + kd * dErr;

194:   lastErr = error;

195:   LOutput = Output - Turn_Speed + Run_Speed;

196:   ROutput = Output + Turn_Speed + Run_Speed;

197: }

198:

199: void MotorControl() { // PWM出力　左右モータの回転方向と出力を制御

200:   if (LOutput > 0) {

201:     ledcWrite(0, min(255, abs(LOutput))); // min()は8ビット出力の最大値255以上をカット

202:     ledcWrite(1, 0);

203:   }

204:   else if (LOutput < 0) {

205:     ledcWrite(0, 0);

206:     ledcWrite(1, min(255, abs(LOutput))); // abs()は-出力を+出力に転換

207:   }

208:   else {

209:     ledcWrite(0, 0);

210:     ledcWrite(1, 0);

211:   }

212:   if (ROutput > 0) {

213:     ledcWrite(2, min(255, abs(ROutput)));

214:     ledcWrite(3, 0);

215:   }

216:   else if (ROutput < 0) {

217:     ledcWrite(2, 0);

218:     ledcWrite(3, min(255, abs(ROutput)));

219:   }

220:   else {

221:     ledcWrite(2, 0);

222:     ledcWrite(3, 0);

223:   }

224:   /*Serial.print("  LOutput: ");

225:   Serial.print(LOutput);

226:   Serial.print(",  ROutput: ");

227:   Serial.println(ROutput);*/

}