254 lines
8.8 KiB
C++
254 lines
8.8 KiB
C++
#include "Dashun.h"
|
||
#include "Dashun_linux.hpp"
|
||
#include "test_nn_20251207_0.hpp"
|
||
#include "test_nn_20251219_0.hpp"
|
||
#include <thread>
|
||
#include <chrono>
|
||
|
||
|
||
|
||
///////////////////////////////////////////////////////////////////
|
||
///////////////////////////////////////////////////////////////////
|
||
/*
|
||
* 适用于机器人的DRL（深度强化学习）
|
||
*
|
||
* 【方案1】
|
||
* 蜘蛛机器人拥有10个舵机（4只胳膊、4只腿、2个摄像云台），它还有2个陀螺仪参数。
|
||
* 将舵机目前的位置+陀螺仪参数传递给神经网络，输出10个舵机的概率值
|
||
* 将概率值加上一个0~1之间的随机因子，获取最终输出
|
||
*
|
||
* 每个舵机180度，即 10 * 180 = 1800 个参数
|
||
*
|
||
* 【方案2】
|
||
*
|
||
* 将蜘蛛机器人当前的陀螺仪读数，以及下一个动作的参数传递给模型
|
||
*
|
||
* 教师数据根据下一个动作的陀螺仪偏差决定
|
||
*/
|
||
|
||
float normalize_pitch(float pitch) {
|
||
return (pitch+3.14) / (2*3.14);
|
||
}
|
||
|
||
float normalize_roll(float roll) {
|
||
return (roll+0.28) / (2* 0.28); /*观测值*/
|
||
}
|
||
|
||
int main() {
|
||
|
||
I2C_init();
|
||
PCA9685_init();
|
||
MPU6050_init();
|
||
|
||
/* 初始化相机 */
|
||
//Camera_init(640, 480);
|
||
//Camera_check();
|
||
//Camera_setVideoFormat();
|
||
//Camera_reqBuf();
|
||
//Camera_setup();
|
||
//Camera_run();
|
||
|
||
|
||
/* 创建归一化器 */
|
||
//ImageLoader image_loader(3, 64, 64, true, false);
|
||
|
||
// 创建网络组件
|
||
//ConvLayer conv1(3, 8, 3, 1, 1); // 输入3通道，输出16通道，3x3卷积，padding=1
|
||
//PoolingLayer pool1(2, 2, 2, 0, true); // 2x2最大池化，步长2
|
||
//ConvLayer conv2(8, 16, 3, 1, 1); // 输入3通道，输出16通道，3x3卷积，padding=1
|
||
//PoolingLayer pool2(2, 2, 2, 0, true); // 2x2最大池化，步长2
|
||
//ConvLayer conv3(16, 32, 3, 1, 1); // 输入16通道，输出32通道
|
||
//PoolingLayer pool3(2, 2, 2, 0, true); // 2x2最大池化，步长2
|
||
//ConvLayer conv4(32, 64, 3, 1, 1); // 输入16通道，输出32通道
|
||
//PoolingLayer pool4(2, 2, 2, 0, true); // 2x2最大池化，步长2
|
||
//ConvLayer conv5(64, 128, 3, 1, 1); // 输入16通道，输出32通道
|
||
//PoolingLayer pool5(2, 2, 2, 0, true); // 2x2最大池化，步长2
|
||
//ConvLayer conv6(128, 256, 3, 1, 1); // 输入16通道，输出32通道
|
||
//PoolingLayer pool6(2, 2, 2, 0, true); // 2x2最大池化，步长2
|
||
//FlattenLayer flatten; // 展平层
|
||
|
||
// 使用CNN构建器
|
||
//CNNBuilder cnn;
|
||
|
||
// 定义输入尺寸
|
||
//int input_channels = 3;
|
||
//int input_height = 64;
|
||
//int input_width = 64;
|
||
//int batch_size = 1;
|
||
|
||
// 构建网络
|
||
//cnn.add_conv_layer(conv1, input_channels, input_height, input_width);
|
||
//cnn.add_pool_layer(pool1, 8, input_height, input_width); // 卷积后尺寸不变（因padding=1）
|
||
//cnn.add_conv_layer(conv2, 8, input_height/2, input_width/2); // 池化后尺寸减半
|
||
//cnn.add_pool_layer(pool2, 16, input_height/2, input_width/2);
|
||
//cnn.add_conv_layer(conv3, 16, input_height/4, input_width/4); // 池化后尺寸减半
|
||
//cnn.add_pool_layer(pool3, 32, input_height/4, input_width/4);
|
||
//cnn.add_conv_layer(conv4, 32, input_height/8, input_width/8); // 池化后尺寸减半
|
||
//cnn.add_pool_layer(pool4, 64, input_height/8, input_width/8);
|
||
//cnn.add_conv_layer(conv5, 64, input_height/16, input_width/16); // 池化后尺寸减半
|
||
//cnn.add_pool_layer(pool5, 128, input_height/16, input_width/16);
|
||
//cnn.add_conv_layer(conv6, 128, input_height/32, input_width/32); // 池化后尺寸减半
|
||
//cnn.add_pool_layer(pool6, 256, input_height/32, input_width/32);
|
||
//cnn.add_flatten_layer(flatten, 256, input_height/64, input_width/64);
|
||
|
||
// 打印网络结构
|
||
//cnn.print_architecture();
|
||
|
||
qing::NNBuilder nn;
|
||
using nnl = NeuralNetwork;
|
||
//auto layer1 = nnl::Create_in_Factory(256, 512, 0.01, nnl::ActivationFunc::Sigmoid);
|
||
//auto layer2 = nnl::Create_in_Factory(512, 256, 0.01, nnl::ActivationFunc::Sigmoid);
|
||
//auto layer3 = nnl::Create_in_Factory(256, 100, 0.01, nnl::ActivationFunc::Sigmoid);
|
||
//auto layer4 = nnl::Create_in_Factory(100, 10, 0.01, nnl::ActivationFunc::Sigmoid);
|
||
//nn.add(layer1);
|
||
//nn.add(layer2);
|
||
//nn.add(layer3);
|
||
//nn.add(layer4);
|
||
|
||
auto layer1 = nnl::Create_in_Factory(18, 36, 0.01, nnl::ActivationFunc::Leaky_ReLU);
|
||
auto layer2 = nnl::Create_in_Factory(36, 36, 0.01, nnl::ActivationFunc::Leaky_ReLU);
|
||
auto layer3 = nnl::Create_in_Factory(36, 36, 0.01, nnl::ActivationFunc::Leaky_ReLU);
|
||
auto layer4 = nnl::Create_in_Factory(36, 16, 0.01, nnl::ActivationFunc::Sigmoid);
|
||
|
||
nn.add(layer1);
|
||
nn.add(layer2);
|
||
nn.add(layer3);
|
||
nn.add(layer4);
|
||
nn.print_shape();
|
||
|
||
|
||
auto parser = qing::ActParser();
|
||
parser.fromStdin();
|
||
auto bot = parser.get();
|
||
while (true) {
|
||
try{
|
||
std::cout << "-----------------------" << std::endl;
|
||
MPU6050_SensorData data = MPU6050_readSensorData();
|
||
float pitch, roll;
|
||
MPU6050_calAngle(data, pitch, roll);
|
||
printf("pitch: %.2f\troll: %.2f\n");
|
||
|
||
/* 获取一个动作 */
|
||
auto act = parser.now();
|
||
auto x = std::vector<float>();
|
||
auto nor_pitch = normalize_pitch(pitch);
|
||
auto nor_roll = normalize_roll(roll);
|
||
x.push_back(nor_pitch);
|
||
x.push_back(nor_roll);
|
||
auto i{0};
|
||
for (; i < act.size()-1; ++i) {
|
||
float a;
|
||
if (bot[i] == 0) { /* 空舵机位 */
|
||
a = 0;
|
||
}
|
||
else {
|
||
a = (float)act[i] / bot[i];
|
||
}
|
||
x.push_back(a);
|
||
std::cout << a << ",";
|
||
}
|
||
std::cout << std::endl;
|
||
|
||
/* 前向反馈 */
|
||
auto x1 = x;
|
||
auto o = nn.forward(x1);
|
||
//auto count1{0}
|
||
//for (auto& item: o) {
|
||
// x[count1] += x[count1] * o * 2 * 0.1; /* 10% */
|
||
// count++;
|
||
//}
|
||
auto j{2};
|
||
auto new_act = std::vector<float>();
|
||
for (; j<x.size(); ++j) {
|
||
auto a = x[j] + o[j-2] * 0.1; /* 10% */
|
||
new_act.push_back((int)(a * bot[j-2]));
|
||
std::cout << a << ",";
|
||
}
|
||
new_act.push_back(act[j-2]);
|
||
std::cout << act[j-2];
|
||
std::cout << std::endl;
|
||
|
||
/* 执行一个动作 */
|
||
parser.next(new_act);
|
||
|
||
MPU6050_SensorData data1 = MPU6050_readSensorData();
|
||
float pitch1, roll1;
|
||
MPU6050_calAngle(data1, pitch1, roll1);
|
||
printf("pitch1: %.2f\troll1: %.2f\n");
|
||
continue;
|
||
|
||
auto err = std::vector<float> ();
|
||
//err.push_back();
|
||
//nor_pitch > 0) /* 计算误差 */
|
||
// ? ((pitch1 > 0) ? nor_pitch - pitch1 : nor_pitch + pitch1 )
|
||
// : ((pitch1 < 0) ? pitch1 - nor_pitch : -(nor_pitch + pitch1));
|
||
|
||
auto count3{0};
|
||
for (auto& item: o) {
|
||
item *= err[count3++];
|
||
}
|
||
|
||
nn.backward(err);
|
||
|
||
}
|
||
|
||
catch(std::runtime_error& exp)
|
||
{
|
||
std::cout << exp.what() << std::endl;
|
||
break;
|
||
}
|
||
}
|
||
|
||
//bool flag = true;
|
||
//while (flag) {
|
||
|
||
//auto img = get_frame_from_camera(flag);
|
||
//auto image_data = image_loader.load_from_cimg(img, true);
|
||
//if (image_data.empty()) {
|
||
// std::cerr << "Failed to load image!" << std::endl;
|
||
// break;
|
||
//}
|
||
|
||
/* 前向传播 */
|
||
//auto output = cnn.forward(image_data, batch_size, true);
|
||
//if (!output.empty()) {
|
||
// std::cout << "Forward pass successful!" << std::endl;
|
||
// std::cout << "Input size: " << image_data.size() << std::endl;
|
||
// std::cout << "Output size: " << output.size() << std::endl;
|
||
|
||
// 计算输出统计信息
|
||
// float sum = 0.0f, max_val = output[0], min_val = output[0];
|
||
// for (float val : output) {
|
||
// sum += val;
|
||
// if (val > max_val) max_val = val;
|
||
// if (val < min_val) min_val = val;
|
||
// }
|
||
|
||
// std::cout << "Output statistics:" << std::endl;
|
||
// std::cout << " Mean: " << sum / output.size() << std::endl;
|
||
// std::cout << " Min: " << min_val << std::endl;
|
||
// std::cout << " Max: " << max_val << std::endl;
|
||
//}
|
||
|
||
//std::vector<float> t(16);
|
||
//auto r = nn.forward(output);
|
||
//auto y = nn.cal_err(r, t);
|
||
|
||
//for (auto i: y) {
|
||
// std::cout << i << " ";
|
||
//}
|
||
//std::cout << std::endl;
|
||
|
||
//y = nn.backward(y);
|
||
//y = cnn.backward(y);
|
||
|
||
//std::this_thread::sleep_for(std::chrono::microseconds(10));
|
||
|
||
|
||
//}
|
||
|
||
//Camera_stop();
|
||
//Camera_unsetup();
|
||
return 0;
|
||
}
|