文章目錄

• 所用硬件
• 程序代碼

所用硬件

原理圖請參考：【UGV】小車一些圖片 麥輪版小車

控制原理請參考：【控制】麥輪小車動力學模型分析

程序代碼

// Motor Controller Use Mecanum Kinematic and Angle Sensor // Author: Zhao-Jichao // Date: 2021-11-29#define IN1 6 // Lower left motor (4) rotates forward #define IN2 7 // Lower left motor (4) rotates backward #define IN3 4 // Upper left moter (3) rotates forward #define IN4 5 // Upper left motor (3) rotates backward #define IN7 10 // Upper right motor (2) rotates forward #define IN8 12 // Upper right motor (2) rotates backward #define IN5 8 // Lower right motor (1) rotates forward #define IN6 9 // Lower right motor (1) rotates backward// JY901 Head File --- #include <Wire.h> #include <JY901.h> // -------------------void setup() {pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT); pinMode(IN5, OUTPUT); pinMode(IN6, OUTPUT);pinMode(IN7, OUTPUT); pinMode(IN8, OUTPUT);pinMode(13, OUTPUT);Serial.begin(9600);Serial3.begin(9600); // Link to JY901，baudrate can't change }void loop() {setMovement(100,-100,0); }void setMovement(float forwBackVel, float leftRightVel, float rotVel) {// Moto compensation to correct move directionint M1Compensate, M2Compensate, M3Compensate, M4Compensate;// --- Start getting yaw angle ---while (Serial3.available()) {JY901.CopeSerialData(Serial3.read()); //Call JY901 data cope function}float yaw = ((float)JY901.stcAngle.Angle[2]/32768*180);Serial.print("Angle:");Serial.println(yaw);// --- End getting yaw angle ---if (yaw>0) {digitalWrite(13, 1);M1Compensate = 00;M2Compensate = 00;M3Compensate = 50;M4Compensate = 50;}else if (yaw<0) {digitalWrite(13, 0);M1Compensate = 50;M2Compensate = 50;M3Compensate = 00;M4Compensate = 00;}float len = 4.5;float wid = 11.5;Moto1(rotVel*(-len-wid) + forwBackVel - leftRightVel + M1Compensate);Moto2(rotVel*(-len-wid) + forwBackVel + leftRightVel + M2Compensate);Moto3(rotVel*( len+wid) + forwBackVel - leftRightVel + M3Compensate);Moto4(rotVel*( len+wid) + forwBackVel + leftRightVel + M4Compensate);}void Moto1(int velocity) {if (velocity > 0){analogWrite(IN5, velocity);analogWrite(IN6, 0);}else if (velocity < 0){analogWrite(IN5, 0);analogWrite(IN6, velocity);}else {digitalWrite(IN5, 1);digitalWrite(IN6, 1);} }void Moto2(int velocity) {if (velocity > 0){analogWrite(IN7, velocity);analogWrite(IN8, 0);}else if (velocity < 0){analogWrite(IN7, 0);analogWrite(IN8, velocity);}else {digitalWrite(IN7, 1);digitalWrite(IN8, 1);} }void Moto3(int velocity) {if (velocity > 0){analogWrite(IN3, velocity);analogWrite(IN4, 0);}else if (velocity < 0){analogWrite(IN3, 0);analogWrite(IN4, velocity);}else {digitalWrite(IN3, 1);digitalWrite(IN4, 1);} }void Moto4(int velocity) {if (velocity > 0){analogWrite(IN1, velocity);analogWrite(IN2, 0);}else if (velocity < 0){analogWrite(IN1, 0);analogWrite(IN2, velocity);}else {digitalWrite(IN1, 1);digitalWrite(IN2, 1);} }

總結

以上是生活随笔為你收集整理的【UGV】Mec 麦轮版小车结合角度传感器实现直线行走示例的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。