#include . // this is for IMU on BLE 33
#include
#include "SensorFusion.h" //SF
SF fusion;
float gx, gy, gz, ax, ay, az, mx, my, mz;
float pitch, roll, yaw;
float deltat;
void setup() {
Serial.begin(115200);
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU!");
while (1);
}
if (!BLE.begin()) {
Serial.println("starting BLE failed!");
while (1);
}
delay(3000);
}
void loop() {
if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable() && IMU.magneticFieldAvailable()) {
IMU.readAcceleration(ax, ay, az);
IMU.readGyroscope(gx, gy, gz);
IMU.readMagneticField(mx, my, mz);
gx = gx - 1.77; //校零
gy = gy + 0.521;
gz = gz + 0.431;
float gyroScale = 3.1415926f / 180;
gx = gx * gyroScale;
gy = gy * gyroScale;
gz = gz * gyroScale;
deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
//choose only one of these two:
// fusion.MahonyUpdate(gx, gy, gz, ax, ay, az, deltat); //mahony is suggested if there isn't the mag and the mcu is slow
fusion.MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat); //else use the magwick, it is slower but more accurate
pitch = fusion.getPitchRadians();
roll = -fusion.getRollRadians(); //you could also use getRollRadians() etc
yaw = fusion.getYawRadians();
Serial.print(pitch);
Serial.print("\t");
Serial.print(roll);
Serial.print("\t");
Serial.print(yaw);
Serial.print("\t");
Serial.print(ax);
Serial.print("\t");
Serial.print(ay);
Serial.print("\t");
Serial.print(az);
Serial.print("\t");
Serial.print(gx);
Serial.print("\t");
Serial.print(gy);
Serial.print("\t");
Serial.println(gz);
}
}
更换思路:放弃通过加速度去计算速度的方法,换种思考角度,尝试通过机器学习来解决这个问题。传感器获得的信息是条件,计算出篮球脱手的速度和角度是方法,是否命中篮筐是结果。既然方法这块弄起来有困难,那么就给足够多的条件和结果,用机器学习的方式去推算方法。
2.38 -3.86 3.05 109.99 -37.11 29.54 2.42 -4 3.15 119.2 -33.75 38.09 2.26 -4 2.83 119.51 -17.82 69.82 2.22 -3.68 2.02 107.6 -6.71 116.46 2.27 -3.05 1.59 89.66 8.48 171.02 1.95 -2.83 1.54 58.29 30.58 244.2 1.38 -2.61 1.4 2.14 63.96 316.47 1.44 -1.88 1.55 -91.19 105.35 365.23 1.13 -2.08 1.97 -211.43 149.72 428.16 0.98 -2.13 2.57 -268.55 260.19 506.71 -0.88 -2.48 2.99 -222.29 360.23 612.37 -3.98 -1.05 1.76 -105.83 356.38 700.93 -4 1.42 0.04 -56.15 430.66 745.18 -4 3.43 -0.39 -177.98 635.07 672.24 -4 4 -1.42 -141.91 619.14 616.46 -4 4 -3.21 -288.51 595.58 332.7 -4 4 -4 -406.19 628.66 125.49 -4 4 -4 -522.77 520.69 56.21 -4 0.99 -4 -439.39 152.71 -34.48 -3.48 -0.33 -3.64 -139.1 -85.88 -300.78 -2.19 -1.99 -3.22 127.01 -62.5 -454.53 -1.21 -3.13 -2.13 189.27 -53.65 -430.85 -0.34 -3.99 -0.93 112.3 -162.72 -322.51 0.73 -4 -0.11 41.26 -242.37 -220.83 0.38 -3.58 -0.02 43.4 -222.53 -187.74 0.73 -3.5 0.26 86 -164.92 -133.06 0.79 -2.52 -0.04 78.37 -105.16 -90.64 1.02 -2.03 0.01 19.78 -78.06 -57.62 0.88 -1.59 0.17 -48.77 -58.78 -32.35 0.68 -1.51 0.28 -69.15 -47.24 -1.16 0.37 -1.23 0.37 -62.87 -32.04 5.86 0.55 -0.65 0.19 -43.46 -17.09 0.85
1.79 -3.44 3.57 128.72 -54.99 59.2 1.82 -3.52 3.43 130.62 -53.96 81.91 1.94 -3.5 3.45 115.05 -29.79 125.55 1.9 -3.65 3.4 61.77 12.21 194.64 1.87 -4 3.33 -20.75 73.36 280.46 1.53 -3.98 3.38 -89.05 135.44 368.29 1.42 -3.27 3.48 -136.35 213.2 437.19 0.76 -2.55 3.62 -207.34 332.76 481.26 -0.45 -2.58 3.97 -256.71 486.88 517.33 -2.33 -2.91 3.57 -205.2 605.41 559.94 -3.98 -2.39 2.84 -46.57 654.91 608.52 -4 0.12 1.42 178.59 715.09 603.09 -4 2.68 1.49 338.62 791.2 533.69 -4 4 0.2 280.09 792.97 479.37 -4 4 0.35 21.12 810.55 411.07 -4 4 -2.11 -226.87 810.73 325.99 -4 4 -2.85 -437.19 638.37 226.56 -4 3.81 -3.64 -456.73 539.25 193.05 -4 1.9 -4 -234.99 430.18 76.11 -3.3 1.32 -3.99 54.44 220.95 -187.19 -2.56 1.23 -3.42 192.75 78.49 -420.78 -2.06 -0.63 -3 201.29 -2.93 -438.6 -1.68 -1.91 -2.19 77.45 -41.56 -441.47 -0.6 -1.38 -1.06 -57.92 -40.95 -428.16 -0.6 -3.58 -0.57 -74.04 -61.77 -305.85 0.2 -2.99 -0.15 -86.43 -70.31 -247.68 0.61 -2.22 -0.05 -79.71 -80.69 -155.76 0.66 -2.09 0.04 -70.31 -90.27 -47.12 0.71 -1.51 -0.1 -83.68 -91.13 -2.38 0.8 -1.3 -0.23 -98.75 -86.55 20.51 0.74 -1.21 -0.36 -106.75 -86.67 50.66 0.76 -0.95 -0.25 -119.08 -52.61 60.97
2.62 -2.9 2.84 88.93 -42.24 47.3 2.69 -3.39 3.11 126.16 -76.9 21.73 3.18 -3.64 3.33 138.73 -68.54 -17.03 3.22 -4 3.3 120.42 -40.16 -19.29 3.09 -4 2.79 92.1 -30.03 31.13 3.16 -4 2.08 70.19 0.79 73.97 2.97 -3.94 2.03 57.13 47.49 137.33 2.68 -3.6 1.6 51.57 85.14 225.22 2 -3.15 1.17 -6.29 139.1 323.67 1.16 -2.27 0.9 -102.48 206.42 446.66 -0.19 -1.81 0.85 -209.66 259.64 559.69 -3.03 0.16 0.19 -266.17 277.34 693.79 -4 1.68 0.02 -266.54 327.45 801.7 -4 3.24 -0.46 -89.54 438.35 758.73 -4 4 -0.79 94.42 535.64 518.8 -4 4 -1.78 134.22 605.1 240.84 -4 4 -2.27 13.67 581.97 -32.35 -4 4 -3.57 -206.18 424.5 -214.17 -4 2.76 -2.58 -392.88 258.06 -284.48 -3.9 -1.8 -2.07 -330.2 86.43 -265.81 -2.04 -3.35 -2.45 -90.52 -12.15 -273.56 -1.13 -2.71 -2.49 96.19 24.6 -300.72 -0.69 -2.12 -2.23 146 32.78 -275.51 -0.13 -1.81 -1.6 102.97 -58.96 -230.96 0.07 -1.88 -0.93 51.15 -168.15 -180.24 0.38 -2.19 -0.32 26.25 -198.18 -147.22 0.57 -2.25 0.07 2.14 -167.18 -141.48 1.04 -2.07 0.34 -25.94 -129.09 -122.62 1.33 -1.93 0.56 -45.41 -96.5 -54.81 0.65 -2.34 0.93 -58.96 -63.05 31.98 0.98 -1.52 0.9 -80.99 -45.78 56.4 0.83 -1.08 0.66 -90.64 -24.11 71.17
0.09 0.99 -0.73 -2.26 37.78 51.51 0.08 0.99 -0.78 0.85 37.41 42.3 0.07 0.97 -0.81 1.1 33.69 35.58 0 1.05 -0.85 0.12 28.87 31.49 -0.1 1.07 -0.88 -0.49 24.54 29.54 -0.2 1.12 -0.88 0.67 23.44 31.13 -0.27 1.15 -0.9 1.34 28.14 36.01 -0.3 1.18 -0.95 -3.66 34.06 44.19 -0.34 1.34 -1.02 -13.49 38.76 50.17 -0.4 1.54 -1.13 -18.98 43.95 51.45 -0.54 1.76 -1.22 -10.68 50.17 47.55 -0.78 1.97 -1.36 4.27 59.14 35.83 -1.16 2.49 -1.54 11.17 67.26 22.46 -1.88 2.92 -1.63 14.89 69.09 10.19 -2.71 3.44 -2.27 8.24 85.33 7.39 -2.96 4 -3.38 -43.33 114.75 8.06 -2.57 4 -4 -68.3 140.93 -1.59 -1.87 4 -4 -39.18 125.18 -47.61 -1.21 3.75 -4 -9.7 71.72 -102.11 -0.84 3.41 -4 -19.78 -32.1 -191.41 -1.33 3.02 -3.61 -10.44 -113.77 -261.78 -0.43 2.38 -3.81 -47.73 -158.75 -331.85 0.03 2.24 -3.86 -142.21 -238.83 -362.61 0.19 1.6 -3.75 -218.08 -355.65 -402.53 0.25 1.03 -3.3 -304.38 -483.89 -446.17 0.36 -0.14 -2.38 -379.7 -635.99 -511.47 -1.18 -0.94 -0.63 -407.04 -847.29 -627.81 -4 -0.38 1.99 -304.02 -1102.17 -790.1 -4 -1.76 4 -14.89 -1289.79 -969.79 -4 -4 4 558.23 -1196.35 -800.78 -4 -4 3.84 759.03 -510.93 -295.84 -3.82 -4 2.76 575.32 -178.83 13.18
-0.72 0.84 -1.08 -0.18 18.19 27.22 -0.69 0.9 -1.11 -1.89 22.64 19.53 -0.7 0.93 -1.11 0.37 27.22 8.36 -0.75 0.94 -1.15 5.55 29.24 -3.17 -0.79 0.91 -1.19 9.22 32.23 -13.31 -0.87 0.88 -1.22 12.08 35.89 -21.42 -0.91 0.88 -1.29 12.94 39.98 -27.95 -1 0.91 -1.37 9.46 44.43 -33.14 -1.11 1.01 -1.47 4.39 48.4 -37.29 -1.33 1.14 -1.57 0.61 45.47 -42.36 -1.71 1.37 -1.74 -0.98 36.93 -48.28 -2.15 1.59 -2.02 1.1 30.58 -53.71 -2.54 1.83 -2.48 0.12 39.43 -61.1 -3.02 2.22 -3.16 -0.92 60.42 -81.79 -3.54 2.64 -3.94 5.92 85.02 -117 -3.67 2.63 -4 16.48 111.27 -156.01 -3.39 2.76 -4 11.84 125.18 -198.43 -2.85 2.68 -4 17.21 108.28 -254.03 -2.21 2.35 -4 26.31 46.57 -322.88 -1.67 2.28 -4 13.98 -37.17 -381.04 -1.61 2.65 -3.99 -20.45 -147.34 -417.48 -1.35 2.3 -3.78 -51.03 -281.56 -453.86 -1.38 3.13 -3.15 -74.46 -424.8 -493.29 -0.88 1.09 -2.76 -58.53 -599.06 -550.11 -1.33 -0.3 -1.79 -76.72 -798.03 -646.42 -2.64 -0.94 -0.48 -171.94 -1031.07 -834.41 -4 -1.61 0.28 -188.78 -1302.12 -975.34 -4 -3.12 0.64 -229.37 -1593.69 -1125.61 -4 -4 4 -267.7 -1458.44 -1121.4 -4 -4 4 -254.15 -523.25 -570.13 -4 -4 4 128.48 -244.81 -61.16 -4 -4 4 302.8 -152.77 280.52
-0.75 1.02 -0.99 -14.83 15.14 41.38 -0.85 1 -0.99 -11.05 13.31 33.63 -0.85 0.99 -0.99 -10.25 18.74 23.19 -0.81 1.01 -1 -9.52 23.25 12.45 -0.86 1.01 -1 -7.81 25.15 3.54 -0.92 1.04 -1.01 -6.9 27.89 -3.36 -0.99 1.06 -1.03 -6.84 31.25 -9.58 -1.05 1.07 -1.06 -7.87 36.32 -15.63 -1.07 1.1 -1.12 -11.54 43.09 -18.92 -1.1 1.16 -1.17 -17.64 50.05 -21.61 -1.25 1.28 -1.24 -19.1 55.79 -26.73 -1.55 1.47 -1.36 -17.33 58.35 -35.52 -2.09 1.91 -1.63 -14.4 59.69 -47.55 -2.89 2.5 -2 -10.01 60.85 -57.5 -3.58 2.87 -2.73 -10.86 64.33 -65.25 -3.8 2.92 -3.47 -22.64 72.2 -84.96 -3.44 2.8 -3.99 -34.97 83.01 -130.25 -2.96 2.74 -4 -35.58 77.76 -202.15 -2.7 2.19 -4 -29.72 34.85 -296.2 -2.42 2.44 -4 -40.65 -40.65 -396.67 -2.21 1.92 -3.96 -38.15 -138.12 -465.76 -2.3 2.57 -3.42 -43.88 -248.9 -514.59 -1.83 2.2 -2.91 -85.33 -356.81 -558.17 -1.44 1.93 -2 -168.4 -484.5 -619.63 -1.27 1.28 -1.42 -253.05 -636.84 -660.16 -1.2 2.03 -0.7 -346.37 -820.8 -734.37 -3.96 -0.65 -0.56 -350.52 -1091.98 -888.18 -4 -0.75 0.63 -308.84 -1445.92 -942.63 -4 0.85 2.89 -228.33 -1602.97 -1137.27 -4 -4 4 60.24 -1350.28 -1002.2 -4 -4 4 185.12 -460.94 -569.82 -4 -4 4 348.94 -176.76 -164.79
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time : 2021/04/21 14:25
# @Author : aramy
# @File : nano33 deal_trandate.py
#处理训练数据
import numpy as np
import pandas as pd
import tensorflow as tf
import tensorflow.keras as keras
dealzero=(4,4,4,2000,2000,2000)*32
def processDate(file,y_val):
openD = pd.read_csv(file, header=None)
dataX = np.array(openD)
if dataX.shape[1]>192:
dataX=dataX[:,:-1]
dataX=dataX/dealzero
dataY=np.ones((dataX.shape[0],))*y_val
return dataX,dataY
def loaddata():
processDate('../res/succ.csv',0) =
processDate('../res/fail.csv', 1) =
dataX = np.concatenate((dataX0, dataX1), axis=0)
dataY=np.append(dataY0,dataY1)
# print(dataX,dataY)
permutationTrain = np.random.permutation(dataX.shape[0])
# # print(permutationTrain)
dataX = dataX[permutationTrain]
dataY = dataY[permutationTrain]
vfoldSize = int(dataX.shape[0] / 100 * 20)
xTest = dataX[0:vfoldSize]
yTest = dataY[0:vfoldSize]
xTrain = dataX[vfoldSize:dataX.shape[0]]
yTrain = dataY[vfoldSize:dataY.shape[0]]
return xTest,yTest,xTrain,yTrain
if __name__ == '__main__':
loaddata() =
xTest,yTest,xTrain,yTrain)
model = keras.Sequential()
input_shape=(6*32,), activation='relu'))
activation='relu'))
activation='softmax'))
adam = keras.optimizers.Adam(0.000005)
'sparse_categorical_crossentropy', optimizer=adam, metrics=['sparse_categorical_accuracy']) =
model.summary()
history = model.fit(xTrain, yTrain, batch_size=1, validation_data=(xTest, yTest), epochs=3000, verbose=1)
converter = tf.lite.TFLiteConverter.from_keras_model(model)
tflite_model = converter.convert()
"wb").write(tflite_model)
Adafruit_SSD1306 display(128, 64, &Wire, NULL);
float ax, ay, az, gx, gy, gz;
float buff[actionnumber * 6];
uint32_t tim[actionnumber];
uint32_t rectime = 0;
const float zero[3] = {0.12620776, -0.360400501, -0.518010013}; //归零
Buff actionBuf;
tflite::MicroErrorReporter tflErrorReporter;
tflite::AllOpsResolver tflOpsResolver;
const tflite::Model* tflModel = nullptr;
tflite::MicroInterpreter* tflInterpreter = nullptr;
TfLiteTensor* tflInputTensor = nullptr;
TfLiteTensor* tflOutputTensor = nullptr;
constexpr int tensorArenaSize = 8 * 1024;
byte tensorArena[tensorArenaSize];
const char* ACTION[] = {
"SUCCESS",
"TOO HARD"
};
float answer[sizeof(ACTION) / sizeof(ACTION[0])];
int action_num=0; //投篮次数
void setup() {
Serial.begin(115200);
tflModel = tflite::GetModel(model);
if (tflModel->version() != TFLITE_SCHEMA_VERSION) {
Serial.println("Model schema mismatch!");
while (1);
}
tflInterpreter = new tflite::MicroInterpreter(tflModel, tflOpsResolver, tensorArena, tensorArenaSize, &tflErrorReporter);
tflInterpreter->AllocateTensors();
tflInputTensor = tflInterpreter->input(0);
tflOutputTensor = tflInterpreter->output(0);
delay(1000);
display.begin(SSD1306_SWITCHCAPVCC, 0x3C); //初始化oled
display.clearDisplay();
display.display();
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU!");
while (1);
}
delay(3000);
}
void loop() {
if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable() ) {
IMU.readAcceleration(ax, ay, az);
IMU.readGyroscope(gx, gy, gz);
actionBuf.insertnode(gx, gy, gz, ax, ay, az, millis());
if (actionBuf.isavailable()) {
actionBuf.getActionVal(buff, tim);
if (millis() - rectime > 1000) {
rectime = millis();
judgeAcion(); //判断是否投中
action_num++;
dispactioninfo(); //显示信息
}
}
}
}
//将收到的数据作推理,判断是否能进球
void judgeAcion() {
float max = 0;
char pos;
//数据归一
for (int i = 0; i << span=""> actionnumber; i++) {
tflInputTensor->data.f[i * 6 + 0] = buff[i * 6 + 0] / 4;
tflInputTensor->data.f[i * 6 + 1] = buff[i * 6 + 1] / 4;
tflInputTensor->data.f[i * 6 + 2] = buff[i * 6 + 2] / 4;
tflInputTensor->data.f[i * 6 + 3] = buff[i * 6 + 3] / 2000;
tflInputTensor->data.f[i * 6 + 4] = buff[i * 6 + 4] / 2000;
tflInputTensor->data.f[i * 6 + 5] = buff[i * 6 + 5] / 2000;
}
TfLiteStatus invokeStatus = tflInterpreter->Invoke();
if (invokeStatus != kTfLiteOk) {
Serial.println("Invoke failed!");
while (1);
return;
}
for (int i = 0; i << span=""> ACT_NUM; i++) {
if (max << span=""> tflOutputTensor->data.f[i]) {
max = tflOutputTensor->data.f[i];
pos = i;
}
Serial.print(ACTION[i]);
Serial.print(": ");
Serial.print(tflOutputTensor->data.f[i], 6);
Serial.print(" ");
answer[i]=tflOutputTensor->data.f[i];
}
Serial.println();
}
//显示结果信息
void dispactioninfo(){
display.clearDisplay(); //清空屏幕
display.setCursor(0, 0); //设置起点
display.setTextSize(2); //设置字体
display.setTextColor(SSD1306_WHITE); //设置字体颜色
display.println(F("NO."));
display.setCursor(60, 0); //设置起点
display.println(action_num); //显示次数
display.setTextSize(3); //设置字体
display.setCursor(0, 24); //设置起点
if(answer[0]>0.8){//投篮成功
display.println("SUCCESS");
}else if(answer[1]>0.8){
display.println("TOO HARD");
}else{
display.println("FAIL");
}
display.setTextSize(1); //设置字体
display.setCursor(0, 48); //设置起点
display.print(answer[0],3);
display.print(" , ");
display.print(answer[1],3);
display.display(); //刷新屏幕
delay(3000);
}
加速度量程应该有点偏小,用到【-8g,8g】不知道效果会不会好点。
采集动作时间长度不够,使用32个动作点,每个动作点之间按9ms计算,才不到300ms。感觉偏小。
需要更多的训练样本,而且最好是实际中产生的训练样本。但是采集样本成本不低,如果大量采集数据,还走串口感觉很不方便,需要学习蓝牙传输数据。
准备还是不够充分。测试中出现了很多问题,USB线松动,魔术贴粘住米奇,开发板脱落……还是需要做好准备。
END
硬禾学堂
硬禾团队一直致力于给电子工程师和相关专业的同学,带来规范的核心技能课程,帮助大家在学习和工作的各个阶段,都能有效地提升自己的职业能力。
硬禾学堂
我们一起在电子领域探索前进
关注硬禾公众号,随时直达课堂
点击阅读原文查看最新一期Funpack