2019NHK_teamA
/
2019_A_ver7-7
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
main.cpp
- Committer:
- yuron
- Date:
- 2019-08-26
- Revision:
- 15:d022288aec51
- Parent:
- 14:ab89b6cd9719
- Child:
- 16:05b26003da50
File content as of revision 15:d022288aec51:
/* ------------------------------------------------------------------- */
/* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic */
/* Nucleo Type: F446RE */
/* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */
/* Sensor: encorder*4 */
/* ------------------------------------------------------------------- */
/* PID関数を各方向毎に追加した */
/* ------------------------------------------------------------------- */
#include "mbed.h"
#include "math.h"
#include "QEI.h"
#include "PID.h"
//PIDGain of wheels
#define Kp 4500000.0
//#define Kp 10000000.0
#define Ti 0.0
#define Td 0.0
PID migimae(Kp, Ti, Td, 0.001);
PID migiusiro(Kp, Ti, Td, 0.001);
PID hidarimae(Kp, Ti, Td, 0.001);
PID hidariusiro(Kp, Ti, Td, 0.001);
//前進
PID front_migimae(4500000.0, 0.0, 0.0, 0.001);
PID front_migiusiro(4500000.0, 0.0, 0.0, 0.001);
PID front_hidarimae(4500000.0, 0.0, 0.0, 0.001);
PID front_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
//後進
PID back_migimae(4500000.0, 0.0, 0.0, 0.001);
PID back_migiusiro(4500000.0, 0.0, 0.0, 0.001);
PID back_hidarimae(4500000.0, 0.0, 0.0, 0.001);
PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
//右進
PID right_migimae(4500000.0, 0.0, 0.0, 0.001);
PID right_migiusiro(4500000.0, 0.0, 0.0, 0.001);
PID right_hidarimae(4500000.0, 0.0, 0.0, 0.001);
PID right_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
//左進
PID left_migimae(4500000.0, 0.0, 0.0, 0.001);
PID left_migiusiro(4500000.0, 0.0, 0.0, 0.001);
PID left_hidarimae(4500000.0, 0.0, 0.0, 0.001);
PID left_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
//右旋回
PID turn_right_migimae(4500000.0, 0.0, 0.0, 0.001);
PID turn_right_migiusiro(4500000.0, 0.0, 0.0, 0.001);
PID turn_right_hidarimae(4500000.0, 0.0, 0.0, 0.001);
PID turn_right_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
//左旋回
PID turn_left_migimae(4500000.0, 0.0, 0.0, 0.001);
PID turn_left_migiusiro(4500000.0, 0.0, 0.0, 0.001);
PID turn_left_hidarimae(4500000.0, 0.0, 0.0, 0.001);
PID turn_left_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
//MDとの通信ポート
I2C i2c(PB_9, PB_8); //SDA, SCL
//PCとの通信ポート
Serial pc(USBTX, USBRX); //TX, RX
//12V停止信号ピン
DigitalOut emergency(D11);
DigitalOut USR_LED1(PC_9);
DigitalOut USR_LED2(PC_8);
DigitalOut USR_LED3(PC_6);
DigitalOut USR_LED4(PC_5);
QEI wheel_x1(PA_8 , PA_6 , NC, 624);
QEI wheel_x2(PB_14, PB_13, NC, 624);
QEI wheel_y1(PB_1 , PB_15, NC, 624);
QEI wheel_y2(PA_12, PA_11, NC, 624);
//QEI wheel1(D2, D3, NC, 624);
//QEI wheel2(D5, D4, NC, 624);
//エンコーダ値格納変数
int x_pulse1, x_pulse2, y_pulse1, y_pulse2;
//操作の段階変数
unsigned int phase = 0;
//MD送信データ変数
char init_send_data[1];
char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1];
char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1];
//関数のプロトタイプ宣言
void init(void);
void init_send(void);
void get_pulses(void);
void print_pulses(void);
void front(unsigned int target);
void back(unsigned int target);
void right(unsigned int target);
void left(unsigned int target);
void turn_right(unsigned int target);
void turn_left(unsigned int target);
void front_PID(unsigned int target);
void back_PID(unsigned int target);
void right_PID(unsigned int target);
void left_PID(unsigned int target);
void turn_right_PID(unsigned int target);
void turn_left_PID(unsigned int target);
void dondonkasoku(void);
int main(void) {
init();
init_send();
while(1) {
get_pulses();
print_pulses();
front(500);
//back(500);
//right(500);
//left(500);
//turn_right(500);
//turn_left(500);
}
}
void init(void) {
//緊急停止用信号ピンをLow
emergency = 0;
//USR_LED1 = 1; USR_LED2 = 1; USR_LED3 = 1; USR_LED4 = 1;
//通信ボーレートの設定
//pc.baud(460800);
pc.baud(9600);
x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 0;
migimae_data[0] = 0x80; migiusiro_data[0] = 0x80; hidarimae_data[0] = 0x80; hidariusiro_data[0] = 0x80;
true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; true_hidariusiro_data[0] = 0x80;
}
void init_send(void) {
init_send_data[0] = 0x80;
i2c.write(0x10, init_send_data, 1);
i2c.write(0x12, init_send_data, 1);
i2c.write(0x14, init_send_data, 1);
i2c.write(0x16, init_send_data, 1);
wait(0.1);
}
void get_pulses(void) {
x_pulse1 = wheel_x1.getPulses();
x_pulse2 = wheel_x2.getPulses();
y_pulse1 = wheel_y1.getPulses();
y_pulse2 = wheel_y2.getPulses();
}
void print_pulses(void) {
pc.printf("x1: %d, x2: %d, y1: %d, y2: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2);
}
void front(unsigned int target) {
front_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
void back(unsigned int target) {
back_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
void right(unsigned int target) {
right_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
void left(unsigned int target) {
left_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
void turn_right(unsigned int target) {
turn_right_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
void turn_left(unsigned int target) {
turn_left_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
void front_PID(unsigned int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
migimae.setInputLimits(-2147483648, 2147483647);
migiusiro.setInputLimits(-2147483648, 2147483647);
hidarimae.setInputLimits(-2147483648, 2147483647);
hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//正転(目標に達してない)
if((y_pulse1 < target) && (y_pulse2 < target)) {
migimae.setOutputLimits(0x84, 0xFF);
migiusiro.setOutputLimits(0x84, 0xFF);
hidarimae.setOutputLimits(0x84, 0xFF);
hidariusiro.setOutputLimits(0x84, 0xFF);
}
//逆転(目標より行き過ぎ)
else if((y_pulse1 > target) && (y_pulse2 > target)) {
migimae.setOutputLimits(0x00, 0x7B);
migiusiro.setOutputLimits(0x00, 0x7B);
hidarimae.setOutputLimits(0x00, 0x7B);
hidariusiro.setOutputLimits(0x00, 0x7B);
} else {
migimae.setOutputLimits(0x7C, 0x83);
migiusiro.setOutputLimits(0x7C, 0x83);
hidarimae.setOutputLimits(0x7C, 0x83);
hidariusiro.setOutputLimits(0x7C, 0x83);
}
//よくわからんやつ
migimae.setMode(AUTO_MODE);
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
//目標値
migimae.setSetPoint(target);
migiusiro.setSetPoint(target);
hidarimae.setSetPoint(target);
hidariusiro.setSetPoint(target);
//センサ出力
migimae.setProcessValue(y_pulse1);
migiusiro.setProcessValue(y_pulse1);
hidarimae.setProcessValue(y_pulse2);
hidariusiro.setProcessValue(y_pulse2);
//制御量(計算結果)
migimae_data[0] = migimae.compute();
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
//制御量をPWM値に変換
//正転(目標に達してない)
if((y_pulse1 < target) && (y_pulse2 < target)) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
}
//逆転(目標より行き過ぎ)
else if((y_pulse1 > target) && (y_pulse2 > target)) {
true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
} else {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
}
void back_PID(unsigned int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
migimae.setInputLimits(-2147483648, 2147483647);
migiusiro.setInputLimits(-2147483648, 2147483647);
hidarimae.setInputLimits(-2147483648, 2147483647);
hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//逆転(目標に達してない)
if((abs(y_pulse1) < target) && (abs(y_pulse2) < target)) {
migimae.setOutputLimits(0x00, 0x7B);
migiusiro.setOutputLimits(0x00, 0x7B);
hidarimae.setOutputLimits(0x00, 0x7B);
hidariusiro.setOutputLimits(0x0, 0x7B);
}
//正転(目標より行き過ぎ)
else if((abs(y_pulse1) > target) && (abs(y_pulse2) > target)) {
migimae.setOutputLimits(0x84, 0xFF);
migiusiro.setOutputLimits(0x84, 0xFF);
hidarimae.setOutputLimits(0x84, 0xFF);
hidariusiro.setOutputLimits(0x84, 0xFF);
} else {
migimae.setOutputLimits(0x7C, 0x83);
migiusiro.setOutputLimits(0x7C, 0x83);
hidarimae.setOutputLimits(0x7C, 0x83);
hidariusiro.setOutputLimits(0x7C, 0x83);
}
//よくわからんやつ
migimae.setMode(AUTO_MODE);
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
//目標値
migimae.setSetPoint(target);
migiusiro.setSetPoint(target);
hidarimae.setSetPoint(target);
hidariusiro.setSetPoint(target);
//センサ出力
migimae.setProcessValue(abs(y_pulse1));
migiusiro.setProcessValue(abs(y_pulse1));
hidarimae.setProcessValue(abs(y_pulse2));
hidariusiro.setProcessValue(abs(y_pulse2));
//制御量(計算結果)
migimae_data[0] = migimae.compute();
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
//制御量をPWM値に変換
//逆転(目標に達してない)
if((abs(y_pulse1) < target) && (abs(y_pulse2) < target)) {
true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
}
//正転(目標より行き過ぎ)
else if((abs(y_pulse1) > target) && (abs(y_pulse2) > target)) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
} else {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
}
void right_PID(unsigned int target) {
//センサ出力値の最小、最大
migimae.setInputLimits(-2147483648, 2147483647);
migiusiro.setInputLimits(-2147483648, 2147483647);
hidarimae.setInputLimits(-2147483648, 2147483647);
hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//右進(目標まで達していない)
if((x_pulse1 < target) && (x_pulse2 < target)) {
migimae.setOutputLimits(0x00, 0x7B);
migiusiro.setOutputLimits(0x84, 0xFF);
hidarimae.setOutputLimits(0x84, 0xFF);
hidariusiro.setOutputLimits(0x00, 0x7B);
}
//左進(目標より行き過ぎ)
else if((x_pulse1 > target) && (x_pulse2 > target)) {
migimae.setOutputLimits(0x84, 0xFF);
migiusiro.setOutputLimits(0x00, 0x7B);
hidarimae.setOutputLimits(0x00, 0x7B);
hidariusiro.setOutputLimits(0x84, 0xFF);
}
//よくわからんやつ
migimae.setMode(AUTO_MODE);
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
//目標値
migimae.setSetPoint(target);
migiusiro.setSetPoint(target);
hidarimae.setSetPoint(target);
hidariusiro.setSetPoint(target);
//センサ出力
migimae.setProcessValue(x_pulse1);
migiusiro.setProcessValue(x_pulse1);
hidarimae.setProcessValue(x_pulse2);
hidariusiro.setProcessValue(x_pulse2);
//制御量(計算結果)
migimae_data[0] = migimae.compute();
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
//制御量をPWM値に変換
//右進(目標まで達していない)
if((x_pulse1 < target) && (x_pulse2 < target)) {
true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
}
//左進(目標より行き過ぎ)
else if((x_pulse1 > target) && (x_pulse2 > target)) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
} else {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
}
void left_PID(unsigned int target) {
//センサ出力値の最小、最大
migimae.setInputLimits(-2147483648, 2147483647);
migiusiro.setInputLimits(-2147483648, 2147483647);
hidarimae.setInputLimits(-2147483648, 2147483647);
hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//左進(目標まで達していない)
if((abs(x_pulse1) < target) && (abs(x_pulse2) < target)) {
migimae.setOutputLimits(0x84, 0xFF);
migiusiro.setOutputLimits(0x00, 0x7B);
hidarimae.setOutputLimits(0x00, 0x7B);
hidariusiro.setOutputLimits(0x84, 0xFF);
}
//右進(目標より行き過ぎ)
else if((abs(x_pulse1) > target) && (abs(x_pulse2) > target)) {
migimae.setOutputLimits(0x00, 0x7B);
migiusiro.setOutputLimits(0x84, 0xFF);
hidarimae.setOutputLimits(0x84, 0xFF);
hidariusiro.setOutputLimits(0x00, 0x7B);
}
//よくわからんやつ
migimae.setMode(AUTO_MODE);
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
//目標値
migimae.setSetPoint(target);
migiusiro.setSetPoint(target);
hidarimae.setSetPoint(target);
hidariusiro.setSetPoint(target);
//センサ出力
migimae.setProcessValue(abs(x_pulse1));
migiusiro.setProcessValue(abs(x_pulse1));
hidarimae.setProcessValue(abs(x_pulse2));
hidariusiro.setProcessValue(abs(x_pulse2));
//制御量(計算結果)
migimae_data[0] = migimae.compute();
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
//制御量をPWM値に変換
//左進(目標まで達していない)
if((abs(x_pulse1) < target) && (abs(x_pulse2) < target)) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
}
//右進(目標より行き過ぎ)
else if((abs(x_pulse1) > target) && (abs(x_pulse2) > target)) {
true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
} else {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
}
void turn_right_PID(unsigned int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
migimae.setInputLimits(-2147483648, 2147483647);
migiusiro.setInputLimits(-2147483648, 2147483647);
hidarimae.setInputLimits(-2147483648, 2147483647);
hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//右旋回(目標に達してない)
if((x_pulse1 < target) && (x_pulse2 < target) && (y_pulse1 < target) && (y_pulse2 < target)) {
migimae.setOutputLimits(0x00, 0x7B);
migiusiro.setOutputLimits(0x00, 0x7B);
hidarimae.setOutputLimits(0x84, 0xFF);
hidariusiro.setOutputLimits(0x84, 0xFF);
}
//左旋回(目標より行き過ぎ)
else if((x_pulse1 > target) && (x_pulse2 > target) && (y_pulse1 > target) && (y_pulse2 > target)) {
migimae.setOutputLimits(0x84, 0xFF);
migiusiro.setOutputLimits(0x84, 0xFF);
hidarimae.setOutputLimits(0x00, 0x7B);
hidariusiro.setOutputLimits(0x00, 0x7B);
}
//よくわからんやつ
migimae.setMode(AUTO_MODE);
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
//目標値
migimae.setSetPoint(target);
migiusiro.setSetPoint(target);
hidarimae.setSetPoint(target);
hidariusiro.setSetPoint(target);
//センサ出力
migimae.setProcessValue(x_pulse1);
migiusiro.setProcessValue(x_pulse2);
hidarimae.setProcessValue(y_pulse1);
hidariusiro.setProcessValue(y_pulse2);
//制御量(計算結果)
migimae_data[0] = migimae.compute();
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
//制御量をPWM値に変換
//右旋回(目標に達してない)
if((x_pulse1 < target) && (x_pulse2 < target) && (y_pulse1 < target) && (y_pulse1 < target)) {
true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
}
//左旋回(目標より行き過ぎ)
else if((x_pulse1 > target) && (x_pulse2 > target) && (y_pulse1 > target) && (y_pulse2 > target)) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
} else {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
}
void turn_left_PID(unsigned int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
migimae.setInputLimits(-2147483648, 2147483647);
migiusiro.setInputLimits(-2147483648, 2147483647);
hidarimae.setInputLimits(-2147483648, 2147483647);
hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//右旋回(目標に達してない)
if((abs(x_pulse1) < target) && (x_pulse2 < target) && (y_pulse1 < target) && (abs(y_pulse2) < target)) {
migimae.setOutputLimits(0x84, 0xFF);
migiusiro.setOutputLimits(0x84, 0xFF);
hidarimae.setOutputLimits(0x00, 0x7B);
hidariusiro.setOutputLimits(0x00, 0x7B);
}
//左旋回(目標より行き過ぎ)
else if((abs(x_pulse1) > target) && (x_pulse2 > target) && (y_pulse1 > target) && (abs(y_pulse2) > target)) {
migimae.setOutputLimits(0x00, 0x7B);
migiusiro.setOutputLimits(0x00, 0x7B);
hidarimae.setOutputLimits(0x84, 0xFF);
hidariusiro.setOutputLimits(0x84, 0xFF);
}
//よくわからんやつ
migimae.setMode(AUTO_MODE);
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
//目標値
migimae.setSetPoint(target);
migiusiro.setSetPoint(target);
hidarimae.setSetPoint(target);
hidariusiro.setSetPoint(target);
//センサ出力
migimae.setProcessValue(abs(x_pulse1));
migiusiro.setProcessValue(abs(y_pulse1));
hidarimae.setProcessValue(abs(y_pulse2));
hidariusiro.setProcessValue(abs(x_pulse2));
//制御量(計算結果)
migimae_data[0] = migimae.compute();
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
//制御量をPWM値に変換
//右旋回(目標に達してない)
if((abs(x_pulse1) < target) && (x_pulse2 < target) && (y_pulse1 < target) && (abs(y_pulse2) < target)) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
}
//左旋回(目標より行き過ぎ)
else if((abs(x_pulse1) > target) && (x_pulse2 > target) && (y_pulse1 > target) && (abs(y_pulse2) > target)) {
true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
} else {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
}
void dondonkasoku(void) {
//どんどん加速(正転)
for(init_send_data[0] = 0x84; init_send_data[0] < 0xFF; init_send_data[0]++){
i2c.write(0x10, init_send_data, 1);
i2c.write(0x12, init_send_data, 1);
i2c.write(0x14, init_send_data, 1);
i2c.write(0x16, init_send_data, 1);
wait(0.05);
}
//どんどん減速(正転)
for(init_send_data[0] = 0xFF; init_send_data[0] >= 0x84; init_send_data[0]--){
i2c.write(0x10, init_send_data, 1);
i2c.write(0x12, init_send_data, 1);
i2c.write(0x14, init_send_data, 1);
i2c.write(0x16, init_send_data, 1);
wait(0.05);
}
//だんだん加速(逆転)
for(init_send_data[0] = 0x7B; init_send_data[0] <= 0x00; init_send_data[0]--){
i2c.write(0x10, init_send_data, 1);
i2c.write(0x12, init_send_data, 1);
i2c.write(0x14, init_send_data, 1);
i2c.write(0x16, init_send_data, 1);
wait(0.05);
}
//だんだん減速(逆転)
for(init_send_data[0] = 0x00; init_send_data[0] <= 0x7B; init_send_data[0]++){
i2c.write(0x10, init_send_data, 1);
i2c.write(0x12, init_send_data, 1);
i2c.write(0x14, init_send_data, 1);
i2c.write(0x16, init_send_data, 1);
wait(0.05);
}
}