Akihiro Nakabayashi
/
MainBoard2018_Auto_Master_A_awasetekudsai
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.
Fork of
MainBoard2018_Auto_Master_A_neww
by 
Taiki Maruyama
System/Process/Process.cpp
- Committer:
- kishibekairohan
- Date:
- 2018-10-06
- Revision:
- 12:028a150943b5
- Parent:
- 11:1295d39fec3a
File content as of revision 12:028a150943b5:
#include "mbed.h"
#include "Process.h"
#include "QEI.h"
#include "../../CommonLibraries/PID/PID.h"
#include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
#include "../../Communication/Controller/Controller.h"
#include "../../Input/ExternalInt/ExternalInt.h"
#include "../../Input/Switch/Switch.h"
#include "../../Input/ColorSensor/ColorSensor.h"
#include "../../Input/AccelerationSensor/AccelerationSensor.h"
#include "../../Input/Potentiometer/Potentiometer.h"
#include "../../Input/Rotaryencoder/Rotaryencoder.h"
#include "../../LED/LED.h"
#include "../../Safty/Safty.h"
#include "../Using.h"
using namespace SWITCH;
using namespace COLORSENSOR;
using namespace ACCELERATIONSENSOR;
using namespace PID_SPACE;
using namespace ROTARYENCODER;
static CONTROLLER::ControllerData *controller;
ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
static bool lock;
static bool processChangeComp;
static int current;
static void AllActuatorReset();
#ifdef USE_SUBPROCESS
static void (*Process[USE_PROCESS_NUM])(void);
#endif
#pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
/*Replace here with the definition code of your variables.*/
Serial pc(USBTX, USBRX);
#define TIRE_FR 0 //足回り前右
#define TIRE_FL 1 //足回り前左
#define TIRE_BR 2 //足回り後右
#define TIRE_BL 3 //足回り後左
#define Angle_R 4 //角度調節右
#define Angle_L 5 //角度調節左
#define Lim_AR 3 //角度調節右
#define Lim_AL 4 //角度調節左
#define Lim_R 0 //センター右
#define Lim_L 1 //センター左
#define EMS_0 LimitSw::IsPressed(8)
#define EMS_1 LimitSw::IsPressed(9)
#define LS LimitSw::IsPressed(7) //赤ゾーン用スイッチ
#define BS LimitSw::IsPressed(6) //青ゾーン用スイッチ
//************メカナム********************
const int mecanum[15][15]=
{
{ 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255},
{ -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255},
{ -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255},
{ -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255},
{ -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255},
{-130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255},
{-187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255},
{-255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255},
{-255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187},
{-255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130},
{-255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83},
{-255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47},
{-255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21},
{-255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5},
{-255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0}
};
const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204};
uint8_t SetStatus(int);
uint8_t SetStatus(int pwmVal){
if(pwmVal < 0) return BACK;
else if(pwmVal > 0) return FOR;
else if(pwmVal == 0) return BRAKE;
else return BRAKE;
}
uint8_t SetPWM(int);
uint8_t SetPWM(int pwmVal){
if(pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
else return abs(pwmVal);
}
//************メカナム********************
//************カラーセンサ********************
int Color_A[3]; //[赤,緑,青]
int Color_B[3];
int Color_C[3];
int Color_D[3];
int intergration = 50;
unsigned long ColorIn(int index)
{
int result = 0;
bool rtn = false;
for(int i=0; i<12; i++)
{
CK[index] = 1;
rtn = DOUT[index];
CK[index] = 0;
if(rtn)
{
result|=(1 << i);
}
}
return result;
}
void ColorDetection();
//************カラーセンサ********************
//************ライントレース変数*******************
int Point[3] = {234, 466, 590};//赤，緑，青
int startP = 35;
int downP = 5;
int Asasult = 0;
int Bsasult = 0;
int Csasult = 0;
int Dsasult = 0;
void pointcalculation();
Ticker Color_T;
//************ライントレース変数*******************
//************ROタコン******************
QEI RtX(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
QEI RtY(RT21_PIN, RT22_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
Ticker get_rpm;
PID Rt_X = PID(0.03, -255, 255, 0.1, 0, 0);
PID Rt_Y = PID(0.03, -255, 255, 0.1, 0, 0);
double rpmX;
double rpmY;
double disX;
double disY;
int palseX;
int palseY;
int RtpwmX;
int RtpwmY;
double goalX = 1200.000;
double goalY = 900.000;
void filip();
//PID startup = PID(0.03, -255, 255, 0.3, 0, 0);
//************ROタコン******************
//************ジャイロ*******************
bool Angle_flagI = false;
int Angle;
PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
float rotateY;
//初期値 -5
int AngletargetX = 4;
int AngletargetY = -12;
int Angle_I = -5;
//************ジャイロ*******************
//************Buzzer******************
//DigitalOut buzzer(BUZZER_PIN);
PwmOut buzzer(BUZZER_PIN);
void BuzzerTimer_func();
Ticker BuzzerTimer;
bool Emsflag = false;
//************Buzzer******************
//************TapeLed*****************
void TapeLedEms_func();
TapeLedData tapeLED;
TapeLedData sendLedData;
TapeLED_Mode ledMode = Normal;
Ticker tapeLedTimer;
//************TapaLed*****************
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0(void);
#endif
#if USE_PROCESS_NUM>1
static void Process1(void);
#endif
#if USE_PROCESS_NUM>2
static void Process2(void);
#endif
#if USE_PROCESS_NUM>3
static void Process3(void);
#endif
#if USE_PROCESS_NUM>4
static void Process4(void);
#endif
#if USE_PROCESS_NUM>5
static void Process5(void);
#endif
#if USE_PROCESS_NUM>6
static void Process6(void);
#endif
#if USE_PROCESS_NUM>7
static void Process7(void);
#endif
#if USE_PROCESS_NUM>8
static void Process8(void);
#endif
#if USE_PROCESS_NUM>9
static void Process9(void);
#endif
#endif
void SystemProcessInitialize()
{
#pragma region USER-DEFINED_VARIABLE_INIT
/*Replace here with the initialization code of your variables.*/
get_rpm.attach_us(&filip,100);
buzzer.period(1.0/800);
#pragma endregion USER-DEFINED_VARIABLE_INIT
lock = true;
processChangeComp = true;
current = DEFAULT_PROCESS;
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
Process[0] = Process0;
#endif
#if USE_PROCESS_NUM>1
Process[1] = Process1;
#endif
#if USE_PROCESS_NUM>2
Process[2] = Process2;
#endif
#if USE_PROCESS_NUM>3
Process[3] = Process3;
#endif
#if USE_PROCESS_NUM>4
Process[4] = Process4;
#endif
#if USE_PROCESS_NUM>5
Process[5] = Process5;
#endif
#if USE_PROCESS_NUM>6
Process[6] = Process6;
#endif
#if USE_PROCESS_NUM>7
Process[7] = Process7;
#endif
#if USE_PROCESS_NUM>8
Process[8] = Process8;
#endif
#if USE_PROCESS_NUM>9
Process[9] = Process9;
#endif
#endif
}
static void SystemProcessUpdate()
{
#ifdef USE_SUBPROCESS
if(controller->Button.HOME) lock = false;
if(controller->Button.START && processChangeComp)
{
current++;
if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
processChangeComp = false;
}
else if(controller->Button.SELECT && processChangeComp)
{
current--;
if (current < 0) current = 0;
processChangeComp = false;
}
else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
#endif
#ifdef USE_MOTOR
ACTUATORHUB::MOTOR::Motor::Update(motor);
#endif
#ifdef USE_SOLENOID
ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
#endif
#ifdef USE_RS485
ACTUATORHUB::ActuatorHub::Update();
#endif
}
void SystemProcess()
{
SystemProcessInitialize();
while(1)
{
#ifdef USE_MU
controller = CONTROLLER::Controller::GetData();
#endif
#ifdef USE_ERRORCHECK
if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
{
CONTROLLER::Controller::DataReset();
AllActuatorReset();
lock = true;
}
else
#endif
{
#ifdef USE_SUBPROCESS
if(!lock)
{
Process[current]();
}
else
#endif
{
//ロック時の処理
}
}
if ((EMS_0 || EMS_1) && !Emsflag){
buzzer = 0.5;
BuzzerTimer.attach(BuzzerTimer_func, 1.2);
Emsflag = true;
ledMode = EMS;
current = 0;
tapeLedTimer.attach(TapeLedEms_func, 1.2);
sendLedData.code = (uint32_t)Red;
}
if(!EMS_0 && !EMS_1) {
buzzer = 0;
BuzzerTimer.detach();
Emsflag = false;
if(ledMode == EMS) ledMode = Normal;
tapeLedTimer.detach();
}
switch(ledMode)
{
case EMS :
break;
case Normal :
sendLedData.code = tapeLED.code;
default:
break;
}
SystemProcessUpdate();
}
}
#pragma region PROCESS
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0()
{
static bool Xnopush = false;
static bool Angle_flagX = false;
if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
motor[Angle_R].pwm = 255;
motor[Angle_L].pwm = 255;
}else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_L].dir == BACK && motor[Angle_L].dir == FOR){
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
motor[Angle_R].pwm = 255;
motor[Angle_L].pwm = 255;
}
for(int i = 0;i<20;i++){
float y = 0;
y = acc[1]*1000;
float rotateY = (y - 305)/2.21 - 90;
Angle += rotateY;
}
Angle = Angle /20;
int gyropwmX = gyro.SetPV(Angle,AngletargetX);
if(controller->Button.X && !Xnopush){
Angle_flagX = true;
Xnopush = true;
}else if(!controller->Button.X)Xnopush = false;
if (Angle_flagX){
motor[Angle_R].dir = SetStatus(-gyropwmX);
motor[Angle_L].dir = SetStatus(gyropwmX);
motor[Angle_R].pwm = SetPWM(gyropwmX);
motor[Angle_L].pwm = SetPWM(gyropwmX);
if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
motor[Angle_R].pwm = 255;
motor[Angle_L].pwm = 255;
Angle_flagX = false;
}
}
else{
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
motor[Angle_R].pwm = 255;
motor[Angle_L].pwm = 255;
}
}
#endif
#if USE_PROCESS_NUM>1
static void Process1()
{
motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]) *0.8;
motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]) *0.8;
motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
motor[TIRE_FR].pwm = motor[TIRE_FR].pwm * 1.3;
motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3;
}
//wheel.getPulses()...どちらの方向にどれだけ回ったか
//pc.printf("Pulses:%07d \r\n",wheel.getPulses());
//軸が何回転したか
//pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
}
#endif
#if USE_PROCESS_NUM>2
static void Process2()
{
static bool color_flag = false;
static bool traceon = false;//fase1
static bool yokofla = false;//fase2
static bool boxslip = false;//fase3
static bool compA = false;
static bool compB = false;
static bool compC = false;
static bool compD = false;
static bool invationA = false;
static bool invationB = false;
static bool invationC = false;
static bool invationD = false;
ColorDetection();
//
if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
{
invationA ^= 1;//start false,over true
compA = true;//on true,noon false
}
else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
{
invationB ^= 1;//start false,over true
compB = true;//on true,noon false
}
else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
/*
if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白
{
invationC ^= 1;//start false,over true
compC = true;//on true,noon false
}
else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2]))compC = false;//茶
if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
{
invationD ^= 1;//start false,over true
compD = true;//on true,noon false
}
else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
*/
//
if(controller->Button.B && !color_flag)
{
traceon ^= 1;
color_flag = true;
}
else if(!controller->Button.B)color_flag = false;
if(traceon && !yokofla && !boxslip)
{
if(!invationA && !compA && !invationB && !compB)
{
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].dir = BACK;
motor[TIRE_FR].pwm = startP;
motor[TIRE_FL].pwm = startP;
motor[TIRE_BR].pwm = startP;
motor[TIRE_BL].pwm = startP;
}
else if(invationA && compA && !invationB && !compB)
{
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].dir = BACK;
motor[TIRE_FR].pwm = startP - downP;
motor[TIRE_FL].pwm = startP - downP;
motor[TIRE_BR].pwm = startP - downP;
motor[TIRE_BL].pwm = startP - downP;
}
else if(invationA && !compA && !invationB && !compB)
{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
wait(2);
yokofla = true;
traceon = false;
}
else{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
}
}
if(!traceon && yokofla && !boxslip)
{
if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
wait(2);
boxslip = true;
yokofla = false;
}
else if(invationA && !compA && invationB)
{
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].dir = FOR;
motor[TIRE_FR].pwm = startP;
motor[TIRE_FL].pwm = startP;
motor[TIRE_BR].pwm = startP;
motor[TIRE_BL].pwm = startP;
}
else if(!invationA && !compB && !invationB)
{
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].dir = BACK;
motor[TIRE_FR].pwm = startP;
motor[TIRE_FL].pwm = startP;
motor[TIRE_BR].pwm = startP;
motor[TIRE_BL].pwm = startP;
}
else if(invationA && compA && !invationB && !compB)
{
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].dir = BACK;
motor[TIRE_FR].pwm = startP;
motor[TIRE_FL].pwm = startP;
motor[TIRE_BR].pwm = startP;
motor[TIRE_BL].pwm = startP;
}
else if(compB && invationB)
{
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].dir = BACK;
motor[TIRE_FR].pwm = startP;
motor[TIRE_FL].pwm = startP;
motor[TIRE_BR].pwm = startP;
motor[TIRE_BL].pwm = startP;
}
else
{
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].dir = BACK;
motor[TIRE_FR].pwm = startP;
motor[TIRE_FL].pwm = startP;
motor[TIRE_BR].pwm = startP;
motor[TIRE_BL].pwm = startP;
}
}
if(!traceon && !yokofla && boxslip)
{
if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
{
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].dir = BACK;
motor[TIRE_FR].pwm = startP;
motor[TIRE_FL].pwm = startP;
motor[TIRE_BR].pwm = startP;
motor[TIRE_BL].pwm = startP;
}
else if(!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))
{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
}
}
/*////
motor[0].dir = BACK;
motor[1].dir = BACK;
motor[2].dir = FOR;
motor[3].dir = FOR;
motor[0].pwm = startP;
motor[1].pwm = startP;
motor[2].pwm = startP;
motor[3].pwm = startP;
else if()
{
motor[0].dir = BRAKE;
motor[1].dir = BRAKE;
motor[2].dir = BRAKE;
motor[3].dir = BRAKE;
motor[0].pwm = 255;
motor[1].pwm = 255;
motor[2].pwm = 255;
motor[3].pwm = 255;
}*///////
}
#endif
#if USE_PROCESS_NUM>3
static void Process3()
{
if(controller->Button.R){
motor[Angle_R].dir = FOR;
motor[Angle_L].dir = BACK;
motor[Angle_R].pwm = 150;
motor[Angle_L].pwm = 150;
}else if(controller->Button.L){
motor[Angle_R].dir = BACK;
motor[Angle_L].dir = FOR;
motor[Angle_R].pwm = 150;
motor[Angle_L].pwm = 150;
}else{
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
motor[Angle_R].pwm = 255;
motor[Angle_L].pwm = 255;
}
if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
motor[Angle_R].pwm = 255;
motor[Angle_L].pwm = 255;
}else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
motor[Angle_R].pwm = 255;
motor[Angle_L].pwm = 255;
}
for(int i = 0;i<20;i++){
float y = 0;
y = acc[1]*1000;
float rotateY = (y - 305)/2.21 - 90;
Angle += rotateY;
}
Angle = Angle/20;
pc.printf("Y:%d \r\n",Angle);
}
#endif
#if USE_PROCESS_NUM>4
static void Process4()
{
static bool Xnopush = false;
static bool Ynopush = false;
static bool Angle_flagX = false;
static bool Angle_flagY = false;
if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
motor[Angle_R].pwm = 255;
motor[Angle_L].pwm = 255;
}else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
motor[Angle_R].pwm = 255;
motor[Angle_L].pwm = 255;
}
for(int i = 0;i<20;i++){
float y = 0;
y = acc[1]*1000;
float rotateY = (y - 305)/2.21 - 90;
Angle += rotateY;
}
Angle = Angle /20;
int gyropwmX = gyro.SetPV(Angle,AngletargetX);
int gyropwmY = gyro.SetPV(Angle,AngletargetY);
if(controller->Button.X && !Xnopush){
Angle_flagX = true;
Xnopush = true;
}else if(!controller->Button.X)Xnopush = false;
if(controller->Button.Y && !Ynopush){
Angle_flagY = true;
Ynopush = true;
}else if(!controller->Button.Y)Ynopush = false;
if (Angle_flagX){
motor[Angle_R].dir = SetStatus(gyropwmX);
motor[Angle_L].dir = SetStatus(-gyropwmX);
motor[Angle_R].pwm = SetPWM(gyropwmX);
motor[Angle_L].pwm = SetPWM(gyropwmX);
if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
motor[Angle_R].pwm = 255;
motor[Angle_L].pwm = 255;
Angle_flagX = false;
}
}
if (Angle_flagY){
motor[Angle_R].dir = SetStatus(-gyropwmY);
motor[Angle_L].dir = SetStatus(gyropwmY);
motor[Angle_R].pwm = SetPWM(gyropwmY);
motor[Angle_L].pwm = SetPWM(gyropwmY);
if(AngletargetY - 2 < Angle && Angle < AngletargetY + 2){
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
motor[Angle_R].pwm = 255;
motor[Angle_L].pwm = 255;
Angle_flagY = false;
}
}
/*float y = 0;
y = acc[1]*1000;
float rotateY = (y - 305)/2.21 - 90;
int gyropwm = gyro.SetPV(rotateY , Angletarget);
if(controller->Button.X){
Angle_flag = true;
}
if (Angle_flag){
motor[Angle_R].dir = SetStatus(gyropwm);
motor[Angle_L].dir = SetStatus(-gyropwm);
motor[Angle_R].pwm = SetPWM(gyropwm);
motor[Angle_L].pwm = SetPWM(gyropwm);
if(Angletarget - 2 < rotateY && rotateY < Angletarget + 2){
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
Angle_flag = false;
}
}*/
else{
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
motor[Angle_R].pwm = 255;
motor[Angle_L].pwm = 255;
}
//pc.printf("%d \r\n",gyropwmY);
}
#endif
#if USE_PROCESS_NUM>5
static void Process5()
{
static bool nopushed = false;
static bool Rt_flagY = false;
/* wait(0.1);
//RtX.getPulses();//...どちらの方向にどれだけ回ったか
//RtY.getPulses();
pc.printf("Pulses:%07d \r\n",RtX.getPulses());
pc.printf("Pulses:%07d \r\n",RtY.getPulses());
//軸が何回転したか
pc.printf("Rotate:%04.3f \r\n",(double)RtX.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
*/
if(controller->Button.B && !nopushed){
Rt_flagY = true;
nopushed = true;
}else if(!controller->Button.B)nopushed = false;
if (Rt_flagY && SetPWM(RtpwmY) > 0){
filip();
motor[TIRE_FR].dir = SetStatus(-RtpwmY);
motor[TIRE_FL].dir = SetStatus(RtpwmY);
motor[TIRE_BR].dir = SetStatus(-RtpwmY);
motor[TIRE_BL].dir = SetStatus(RtpwmY);
motor[TIRE_FR].pwm = SetPWM(RtpwmY);
motor[TIRE_FL].pwm = SetPWM(RtpwmY);
motor[TIRE_BR].pwm = SetPWM(RtpwmY);
motor[TIRE_BL].pwm = SetPWM(RtpwmY);
}
else if(Rt_flagY && SetPWM(RtpwmY) < 0)
{
filip();
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_FR].pwm = 255;
motor[TIRE_FL].pwm = 255;
motor[TIRE_BR].pwm = 255;
motor[TIRE_BL].pwm = 255;
}
else
{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_FR].pwm = 255;
motor[TIRE_FL].pwm = 255;
motor[TIRE_BR].pwm = 255;
motor[TIRE_BL].pwm = 255;
}
//pc.printf("RtpwmX:%f \r\n", RtpwmX);
//pc.printf("PWM:%d \r\n", RtpwmY);
//pc.printf("回転数:%f \r\n" ,rpmY);
//pc.printf("距離:%f \r\n", disY);
}
#endif
#if USE_PROCESS_NUM>6
static void Process6()
{
/*static bool color_flag = false;
static bool traceon = false;//fase1
static bool yokofla = false;//fase2
static bool boxslip = false;//fase3
static bool syu = false;
static bool compA = false;
static bool compB = false;
static bool compC = false;
static bool compD = false;
static bool invationA = false;
static bool invationB = false;
static bool invationC = false;
static bool invationD = false;
ColorDetection();
//
if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
{
invationA ^= 1;//start false,over true
compA = true;//on true,noon false
}
else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
{
invationB ^= 1;//start false,over true
compB = true;//on true,noon false
}
else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
/*
if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白
{
invationC ^= 1;//start false,over true
compC = true;//on true,noon false
}
else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2]))compC = false;//茶
if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
{
invationD ^= 1;//start false,over true
compD = true;//on true,noon false
}
else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
*/
//
/*
if(controller->Button.B && !color_flag)
{
traceon ^= 1;
color_flag = true;
}
else if(!controller->Button.B)color_flag = false;
if(traceon && !yokofla && !boxslip && !syu)
{
if(!invationA && !compA && !invationB && !compB)
{
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].dir = BACK;
motor[TIRE_FR].pwm = startP;
motor[TIRE_FL].pwm = startP;
motor[TIRE_BR].pwm = startP;
motor[TIRE_BL].pwm = startP;
}
else if(invationA && compA && !invationB && !compB)
{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
wait(2);
syu = true;
yokofla = false;
traceon = false;
}
else{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
}
}
pointcalculation();
if(syu && !traceon && !yokofla && !boxslip)
{
if(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10)
{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
wait(2);
yokofla = true;
traceon = false;
syu = false;
}
else if(Asasult < Bsasult && !(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10))
{
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].dir = FOR;
motor[TIRE_FR].pwm = startP;
motor[TIRE_FL].pwm = startP;
motor[TIRE_BR].pwm = startP;
motor[TIRE_BL].pwm = startP;
}
else if(Asasult > Bsasult && !(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10))
{
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].dir = BACK;
motor[TIRE_FR].pwm = startP;
motor[TIRE_FL].pwm = startP;
motor[TIRE_BR].pwm = startP;
motor[TIRE_BL].pwm = startP;
}
else
{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
}
}
if(!syu && !traceon && yokofla && !boxslip)
{
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].dir = BACK;
motor[TIRE_FR].pwm = startP;
motor[TIRE_FL].pwm = startP;
motor[TIRE_BR].pwm = startP;
motor[TIRE_BL].pwm = startP;
}
else if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
wait(2);
boxslip = true;
yokofla = false;
}
else
{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
}
*/
}
#endif
#if USE_PROCESS_NUM>7
static void Process7()
{
static bool nopushed = false;
static bool Rt_flagX = false;
if(controller->Button.A && !nopushed){
Rt_flagX = true;
nopushed = true;
}else if(!controller->Button.A)nopushed = false;
if (Rt_flagX && SetPWM(RtpwmX) > 0){
filip();
motor[TIRE_FR].dir = SetStatus(-RtpwmX);
motor[TIRE_FL].dir = SetStatus(-RtpwmX);
motor[TIRE_BR].dir = SetStatus(RtpwmX);
motor[TIRE_BL].dir = SetStatus(RtpwmX);
motor[TIRE_FR].pwm = SetPWM(RtpwmX);
motor[TIRE_FL].pwm = SetPWM(RtpwmX);
motor[TIRE_BR].pwm = SetPWM(RtpwmX);
motor[TIRE_BL].pwm = SetPWM(RtpwmX);
}
else if(Rt_flagX && SetPWM(RtpwmX) < 0)
{
filip();
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_FR].pwm = 255;
motor[TIRE_FL].pwm = 255;
motor[TIRE_BR].pwm = 255;
motor[TIRE_BL].pwm = 255;
}
else
{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_FR].pwm = 255;
motor[TIRE_FL].pwm = 255;
motor[TIRE_BR].pwm = 255;
motor[TIRE_BL].pwm = 255;
}
//pc.printf("X:%d \r\n",RtpwmX);
}
#endif
#if USE_PROCESS_NUM>8
static void Process8()
{
}
#endif
#if USE_PROCESS_NUM>9
static void Process9()
{
}
#endif
#endif
#pragma endregion PROCESS
static void AllActuatorReset()
{
#ifdef USE_SOLENOID
solenoid.all = ALL_SOLENOID_OFF;
#endif
#ifdef USE_MOTOR
for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
{
motor[i].dir = FREE;
motor[i].pwm = 0;
}
#endif
}
#pragma region USER-DEFINED-FUNCTIONS
void pointcalculation(){
ColorDetection();
/*if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
{
invationA ^= 1;//start false,over true
compA = true;//on true,noon false
}
else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶*/
for(int i=0;i<3;i++){Asasult += Color_A[i]-Point[i];}
for(int i=0;i<3;i++){Bsasult += Color_B[i]-Point[i];}
for(int i=0;i<3;i++){Csasult += Color_A[i]-Point[i];}
for(int i=0;i<3;i++){Dsasult += Color_B[i]-Point[i];}
}
void filip(){
palseX = RtX.getPulses();
palseY = RtY.getPulses();
rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
disX = 48*3.141*rpmX;
disY = 48*3.141*rpmY;
RtpwmX = (int)Rt_X.SetPV(disX , goalX);
RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
}
void ColorDetection(){
GATE = 0;
CK[0] = 0;
CK[1] = 0;
CK[2] = 0;
CK[3] = 0;
RANGE = 1;
GATE = 1;
wait_ms(intergration);
GATE = 0;
wait_us(4);
Color_A[0] = ColorIn(0); //赤
wait_us(3);
Color_A[1] = ColorIn(0); //青
wait_us(3);
Color_A[2] = ColorIn(0); //緑
Color_B[0] = ColorIn(1);
wait_us(3);
Color_B[1] = ColorIn(1);
wait_us(3);
Color_B[2] = ColorIn(1);
Color_C[0] = ColorIn(2);
wait_us(3);
Color_C[1] = ColorIn(2);
wait_us(3);
Color_C[2] = ColorIn(2);
Color_D[0] = ColorIn(3);
wait_us(3);
Color_D[1] = ColorIn(3);
wait_us(3);
Color_D[2] = ColorIn(3);
}
void BuzzerTimer_func() {
if(buzzer == 0.5){
buzzer = 0;
}
else if(buzzer == 0){
buzzer = 0.5;
}
}
void TapeLedEms_func() {
sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
}
#pragma endregion