大季 矢花
aa
Diff: System/Process/Process.cpp
- Revision:
- 22:7d93f79a3686
- Parent:
- 21:e3b58d675c1c
- Child:
- 23:c853372cf626
--- a/System/Process/Process.cpp Mon Sep 09 00:19:28 2019 +0000
+++ b/System/Process/Process.cpp Tue Sep 17 04:40:17 2019 +0000
@@ -1,7 +1,6 @@
#include "mbed.h"
#include "Process.h"
-#include "QEI.h"
#include "../../CommonLibraries/PID/PID.h"
#include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
@@ -9,8 +8,8 @@
#include "../../Communication/Controller/Controller.h"
#include "../../Input/ExternalInt/ExternalInt.h"
#include "../../Input/Switch/Switch.h"
-#include "../../Input/Potentiometer/Potentiometer.h"
#include "../../Input/Encoder/Encoder.h"
+#include "../../Input/Ultrasonic/Ultrasonic.h"
#include "../../LED/LED.h"
#include "../../Safty/Safty.h"
#include "../Using.h"
@@ -18,8 +17,10 @@
using namespace SWITCH;
using namespace PID_SPACE;
using namespace ENCODER;
+using namespace ULTRASONIC;
using namespace LINEHUB;
+
static CONTROLLER::ControllerData *controller;
ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
@@ -38,15 +39,12 @@
/*Replace here with the definition code of your variables.*/
-Serial pc(USBTX, USBRX);
+USS ultrasonic[] = {
+ USS(ECHO_0,TRIG_0,TEMP),
+ USS(ECHO_1,TRIG_1,TEMP),
+ };
-//**************Encoder***************
-const int PerRev = 256;
-QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING);
-QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING);
-QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING);
-QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING);
-//**************Encoder***************
+Serial pc(USBTX, USBRX);
//**************Buzzer****************
//DigitalOut buzzer(BUZZER_PIN);
@@ -64,6 +62,95 @@
Ticker tapeLedTimer;
//************TapaLed*****************
+//*************** lift ***************
+#define LOWER 1
+#define MIDDLRE 2
+#define UPPER 3
+uint8_t liftState = LOWER;
+bool moving = false;
+bool switchFlag_LB = false;
+bool switchFlag_RB = false;
+
+//*************** lift ***************
+
+//*************tire*************
+PID rotaconPID[] = {
+ PID(0.0001,-1,1,0.05,0,0), //LF
+ PID(0.0001,-1,1,0.05,0,0), //LB
+ PID(0.0001,-1,1,0.05,0,0), //RB
+ PID(0.0001,-1,1,0.05,0,0), //RF
+};
+
+#define FL 0
+#define BL 1
+#define BR 2
+#define FR 3
+
+#define PI 3.141592
+
+const float tireR = 101.6; //タイヤの半径
+const float ucR = 420.0; //中心からのタイヤの距離
+
+typedef struct {
+ float Vx; //X方向の速度
+ float Vy; //Y方向の速度
+ float Va; //角速度
+} Vvector;
+
+Vvector move; //進む速度
+Vvector correction_LT; //ライントレースの補正速度
+Vvector synthetic; //合成速度
+
+float sita = 0;
+
+bool PIDflag = false;
+
+int linePara[8];
+int linePara_U;
+int linePara_B;
+int linePara_L;
+int linePara_R;
+
+#define FL 0
+#define BL 1
+#define BR 2
+#define FR 3
+
+float tireProcessRPM[4];
+float tireTargetMaxRPM[4];
+float tireTargetRPM[4];
+
+float tirePWM[4];
+
+float timePV[4];
+float timeCV[4];
+float pulsePV[4];
+float pulseCV[4];
+
+void tirePID();
+int lineCast(char k);
+
+Timer rotaconSampling;
+Ticker rotaconPIDtimer;
+
+bool countFlag;
+//*************tire**************//
+
+// ************* Line ************** //
+
+float pw = 0;
+int lineFase = 0;
+bool lineCheck = false;
+int linePWM;
+int adj_F;
+int adj_B;
+
+int mode = 0;
+
+int lineCount = 0;
+
+// ************* Line ************** //
+
const int omni[15][15] =
{
{ 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
@@ -137,7 +224,7 @@
{
#pragma region USER-DEFINED_VARIABLE_INIT
/*Replace here with the initialization code of your variables.*/
-
+ rotaconPIDtimer.attach(tirePID,0.1);
#pragma endregion USER-DEFINED_VARIABLE_INIT
@@ -224,9 +311,17 @@
{
int g[8];
for(int i = 0; i < 8; i++){
- g[i] = LineHub::GetPara(i);
+ g[i] = lineCast(LineHub::GetPara(i));
}
- printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
+
+ pc.printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
+
+ //float a = ultrasonic[0].ReadDis();
+ //pc.printf("%f\n\r",a);
+
+ //int ppap = encoder[0].getPulses();
+ //pc.printf("%d\n\r",ppap);
+
buzzer.period(1.0/800);
@@ -257,7 +352,9 @@
}
}
+
//Emergency!
+ /*
if(!EMG_0 && !EMG_1 && !EMGflag){
buzzer = 0;
BuzzerTimer.attach(BuzzerTimer_func, 1);
@@ -269,6 +366,7 @@
BuzzerTimer.detach();
EMGflag = false;
}
+ */
SystemProcessUpdate();
}
}
@@ -288,7 +386,9 @@
#if USE_PROCESS_NUM>1
static void Process1()
{
- /*
+
+ PIDflag = false;
+
if(controller->Button.UP) {
motor[LIFT_LB].dir = FOR;
motor[LIFT_LB].pwm = 180;
@@ -305,7 +405,7 @@
motor[LIFT_RB].dir = BRAKE;
motor[LIFT_RB].pwm = 200;
}
- */
+
if(controller->Button.X) {
motor[LIFT_U].dir = FOR;
@@ -324,10 +424,10 @@
motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] );
motor[TIRE_FR].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] );
- motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]) ;
- motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]) ;
- motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]) ;
- motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y]) ;
+ motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
+ motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2) ;
+ motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
+ motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
} else {
motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
@@ -345,28 +445,219 @@
#if USE_PROCESS_NUM>2
static void Process2()
{
-
+/*
+ if(moving) {
+ if(LimiSw::IsPressed(LSW_LB)) {
+ if(switchFlag_LB) {
+ switchFlag_LB = false;
+ motor[LIFT_LB].dir = BRAKE;
+ motor[LIFT_LB].pwm = 200;
+ } else {
+ seitchFlag_LB = true;
+ }
+ }
+ if(LimiSw::IsPressed(LSW_RB)) {
+ if(switchFlag_RB) {
+ switchFlag_RB = false;
+ motor[LIFT_RB].dir = BRAKE;
+ motor[LIFT_RB].pwm = 200;
+ } else {
+ seitchFlag_RB = true;
+ }
+ }
+ if(motor[LIFT_LB].dir == BRAKE && motor[LIFT_RB].dir == BRAKE) moving = false;
+ } else {
+ if(controller->Button.UP) {
+ if(!(state == UPPER)) {
+ state++;
+ motor[LIFT_LB].dir = BACK;
+ motor[LIFT_RB].dir = FOR;
+ motor[LIFT_LB].pwm = 200;
+ motor[LIFT_RB].pwm = 200;
+ }
+ } else if(controller->Button.DOWN) {
+ if(!(state == LOWER)) {
+ state--;
+ moving = true;
+ motor[LIFT_LB].dir = FOR;
+ motor[LIFT_RB].dir = BACK;
+ motor[LIFT_LB].pwm = 200;
+ motor[LIFT_RB].pwm = 200;
+ }
+ } else {
+ motor[LIFT_LB].dir = BRAKE;
+ motor[LIFT_RB].dir = BRAKE;
+ motor[LIFT_LB].pwm = 200;
+ motor[LIFT_RB].pwm = 200;
+ }
+ }
+*/
}
#endif
#if USE_PROCESS_NUM>3
static void Process3()
{
-
+ AllActuatorReset();
}
#endif
#if USE_PROCESS_NUM>4
static void Process4()
-{
+{
+
+ static int x,y;
+ static int count = 0;
+
+ linePara_U = lineCast(LineHub::GetPara(0));
+ linePara_B = lineCast(LineHub::GetPara(2));
+ linePara_L = lineCast(LineHub::GetPara(3));
+ linePara_R = lineCast(LineHub::GetPara(4));
+
+ if(linePara_U == 'A' && count == 0) {
+ lineFase = 1;
+ }
+ if(lineFase == 0) {
+ pw = 0.55;
+ switch(linePara_U) {
+ case -2:
+ x = 5;
+ y = 3;
+ break;
+ case -3:
+ x = 5;
+ y = 3;
+ break;
+ case -1:
+ x = 6;
+ y = 3;
+ break;
+ case 0:
+ x = 7;
+ y = 3;
+ break;
+ case 1:
+ x = 8;
+ y = 3;
+ break;
+ case 3:
+ x = 9;
+ y = 3;
+ break;
+ case 2:
+ x = 9;
+ y = 3;
+ break;
+ case 'A':
+ lineCheck = true;
+ x = x;
+ y = y;
+ break;
+ case 'N':
+ x = 7;
+ y = 7;
+ break;
+ x = 7;
+ y = 7;
+ default:
+ x = 9;
+ y = 9;
+ }
+ if(lineCheck == true && (!(linePara_U) == 'A')) {
+ count++;
+ }
+ } else if(lineFase == 1) {
+ pw = 0.4;
+ switch(linePara_B) {
+ case -2:
+ x = 5;
+ y = 3;
+ break;
+ case -3:
+ x = 5;
+ y = 3;
+ break;
+ case -1:
+ x = 6;
+ y = 3;
+ break;
+ case 0:
+ x = 7;
+ y = 3;
+ break;
+ case 1:
+ x = 8;
+ y = 3;
+ break;
+ case 3:
+ x = 9;
+ y = 3;
+ break;
+ case 2:
+ x = 9;
+ y = 3;
+ break;
+ case 'A':
+ x = 7;
+ y = 7;
+ break;
+ case 'N':
+ x = 7;
+ y = 7;
+ break;
+ x = 7;
+ y = 7;
+ default:
+ x = 9;
+ y = 9;
+ }
+ if(linePara_R == 0 && linePara_L == 0) {
+ lineFase = 2;
+ x = 7;
+ y = 7;
+ }
+ } else if(lineFase == 2) {
+ x = 7;
+ y = 7;
+ } else {
+ x = 7;
+ y = 7;
+ }
+
+ int t = 0;
+ if((linePara_U + linePara_B) > 3) t = 1;
+
+ if(controller->Button.A) {
+ motor[TIRE_BL].dir = SetStatus(-omni[y][14-x] );
+ motor[TIRE_FL].dir = SetStatus(omni[y][x]);
+ motor[TIRE_BR].dir = SetStatus(-omni[14-x][14-y]);
+ motor[TIRE_FR].dir = SetStatus(omni[x][14-y]);
+
+ motor[TIRE_FR].pwm = SetPWM((omni[y][14-x])) * pw;
+ motor[TIRE_FL].pwm = SetPWM((omni[y][x])) * pw;
+ motor[TIRE_BR].pwm = SetPWM((omni[14-x][14-y])) * pw;
+ motor[TIRE_BL].pwm = SetPWM((omni[x][14-y])) * pw;
+ } else {
+ motor[TIRE_BL].dir = SetStatus(0);
+ motor[TIRE_FL].dir = SetStatus(0);
+ motor[TIRE_BR].dir = SetStatus(0);
+ motor[TIRE_FR].dir = SetStatus(0);
+
+ motor[TIRE_FR].pwm = SetPWM(0);
+ motor[TIRE_FL].pwm = SetPWM(0);
+ motor[TIRE_BR].pwm = SetPWM(0);
+ motor[TIRE_BL].pwm = SetPWM(0);
+ }
+
}
#endif
#if USE_PROCESS_NUM>5
static void Process5()
{
-
+ lineFase = 0;
+ lineCheck = true;
}
#endif
@@ -374,20 +665,295 @@
static void Process6()
{
+ for(int i = 0; i < 8; i++){
+ linePara[i] = lineCast(LineHub::GetPara(i));
+ }
+
+ static int count = 0;
+ count++;
+
+ if(count < 10000) {
+ lineCheck = false;
+ } else {
+ lineCheck = true;
+ }
+
+ if(lineFase == 0) { // 前進
+ switch(linePara[0]) {
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BL].pwm = 0;
+ break;
+ case -3:
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FR].pwm = 10;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BL].pwm = 10;
+ break;
+ case -1:
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BL].pwm = 20;
+ break;
+ case 0:
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BL].pwm = 30;
+ break;
+ case 1:
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BL].pwm = 30;
+ break;
+ case 3:
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 10;
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_BR].pwm = 10;
+ motor[TIRE_BL].pwm = 30;
+ break;
+ case 2:
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BL].pwm = 30;
+ break;
+ case 'A':
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BL].pwm = 30;
+ if(lineCheck == true) {
+ lineCount++;
+ count = 0;
+ }
+ default:
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BL].pwm = 30;
+ }
+ if(lineCount == 1) {
+ lineFase = 1;
+ }
+ } else if(lineFase == 1) { // 前進 低速
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FR].pwm = 15;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BL].pwm = 15;
+ if(linePara[4] == 0) {
+ lineFase = 2;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BL].pwm = 30;
+ }
+ } else if(lineFase == 2){ // 位置調整
+ lineFase = 3;
+ } else if(lineFase == 3){ // 右 直進
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BL].pwm = 30;
+ }
+
}
#endif
#if USE_PROCESS_NUM>7
static void Process7()
{
-
+ if(lineFase == 0) { // 前進
+ switch(linePara[0]) {
+ case -2: adj_F = 10;
+ break;
+ case -3: adj_F = 5;
+ break;
+ case -1: adj_F = 2;
+ break;
+ case 0: adj_F = 0;
+ break;
+ case 1: adj_F = -2;
+ break;
+ case 3: adj_F = -5;
+ break;
+ case 2: adj_F = -10;
+ break;
+ case 'A':
+ break;
+ case 'N':
+ break;
+ default:
+ }
+ switch(linePara[2]) {
+ case -2: adj_F = -10;
+ break;
+ case -3: adj_F = -5;
+ break;
+ case -1: adj_F = -2;
+ break;
+ case 0: adj_F = 0;
+ break;
+ case 1: adj_F = 2;
+ break;
+ case 3: adj_F = 5;
+ break;
+ case 2: adj_F = 10;
+ break;
+ case 'A': adj_F = 0;
+ break;
+ case 'N':
+ break;
+ default:
+ }
+
+ if(mode == 0) linePWM = 40;
+ else if (mode == 1) linePWM = 20;
+ else if (mode == 2) linePWM = 0;
+ else linePWM = 0;
+
+ motor[TIRE_FL].dir = SetStatus(linePWM - adj_F);
+ motor[TIRE_BL].dir = SetStatus(linePWM - adj_B);
+ motor[TIRE_BR].dir = SetStatus(-linePWM - adj_B);
+ motor[TIRE_FR].dir = SetStatus(-linePWM - adj_F);
+ motor[TIRE_FR].pwm = SetPWM(linePWM - adj_F);
+ motor[TIRE_FL].pwm = SetPWM(linePWM - adj_B);
+ motor[TIRE_BR].pwm = SetPWM(-linePWM - adj_B);
+ motor[TIRE_BL].pwm = SetPWM(-linePWM - adj_F);
+ }
}
#endif
#if USE_PROCESS_NUM>8
static void Process8()
{
-
+ if(controller->Button.A) {
+ rotaconSampling.start();
+ PIDflag = true;
+
+ //linePara_U = LineHub::GetPara(0);
+ //linePara_B = LineHub::GetPara(3);
+
+
+ pulsePV[FL] = encoder[FL].getPulses();
+ pulsePV[BL] = encoder[BL].getPulses();
+ pulsePV[BR] = encoder[BR].getPulses();
+ pulsePV[FR] = encoder[FR].getPulses();
+
+
+ for (int i = 0; i < 4; i++) {
+ timeCV[i] = timePV[i];
+ timePV[i] = rotaconSampling.read();
+ tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60;
+ pulseCV[i] = pulsePV[i];
+ }
+
+ move.Vx = 0.5;
+ move.Vy = 0.5;
+ move.Va = 0;
+
+ correction_LT.Vx = 0; //0.1 * linePara_U;
+ correction_LT.Vy = 0;
+ correction_LT.Va = 0;
+
+ synthetic.Vx = move.Vx + correction_LT.Vx;
+ synthetic.Vy = move.Vy + correction_LT.Vy;
+ synthetic.Va = move.Va + correction_LT.Va;
+
+ sita = 0;
+
+ //タイヤの目標速度算出
+ float sinR = 0.7071 * (float)sin(sita);
+ float cosR = 0.7071 * (float)cos(sita);
+ float nv = (60 * 1000) / ( 2.00 * PI * tireR);
+ tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
+ tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
+ tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
+ tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
+
+ //pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]);
+
+ //PIDによるPWM算出
+
+ //モータの駆動
+ for (int i = 0; i < 4; i++) {
+ if (tirePWM[i] > 255){
+ tirePWM[i] = 255;
+ } else if (tirePWM[i] < -255) {
+ tirePWM[i] = -255;
+ }
+ }
+
+ for(int i = 0;i < 4;i++){
+ motor[i].dir = SetStatus(tirePWM[i]);
+ motor[i].pwm = SetPWM(tirePWM[i]);
+ }
+ } else {
+ PIDflag = false;
+ rotaconSampling.stop();
+ rotaconSampling.reset();
+ for(int i = 0;i < 4;i++){
+ encoder[i].reset();
+ pulsePV[i] = 0;
+ pulseCV[i] = 0;
+ timePV[i] = 0;
+ timeCV[i] = 0;
+ tirePWM[i] = 0;
+ motor[i].dir = SetStatus(tirePWM[i]);
+ motor[i].pwm = SetPWM(tirePWM[i]);
+ }
+ }
}
#endif
@@ -426,6 +992,37 @@
}
#pragma region USER-DEFINED-FUNCTIONS
+void tirePID() {
+ if(PIDflag == true) {
+ //加算するPID値の算出
+ rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]);
+ rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]);
+ rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]);
+ rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]);
+ //PID値の加算
+ tirePWM[FL] += rotaconPID[0].GetMV();
+ tirePWM[BL] += rotaconPID[1].GetMV();
+ tirePWM[FR] += rotaconPID[2].GetMV();
+ tirePWM[BR] += rotaconPID[3].GetMV();
+ }
+}
+int lineCast(char k) {
+ int l;
+ switch(k) {
+ case 255:
+ l = -1;
+ break;
+ case 254:
+ l = -2;
+ break;
+ case 253:
+ l = -3;
+ break;
+ default:
+ l = k;
+ }
+ return l;
+}
#pragma endregion