Miya Miyagawa
lknds
Dependencies: mbed TrapezoidControl Pulse QEI
System/Process/Process.cpp
- Committer:
- kishibekairohan
- Date:
- 2018-10-05
- Revision:
- 10:1295d39fec3a
- Parent:
- 9:f93fc79a49ea
- Child:
- 11:028a150943b5
File content as of revision 10:1295d39fec3a:
#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);
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;
}
#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)
//************メカナム********************
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;
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;
float Angle;
PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
float rotateY;
int AngletargetX = 18;
int AngletargetY = -35;
int Angle_I = -5;
//************ジャイロ*******************
//************Buzzer******************
DigitalOut buzzer(BUZZER_PIN);
void BuzzerTimer_func();
Ticker BuzzerTimer;
bool Emsflag = false;
//************Buzzer******************
#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
get_rpm.attach_us(&filip,100);
/*Replace here with the initialization code of your variables.*/
#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)
{
/*get_rpm.attach_us(&filip,1000);
disX = 48*3.141592*rpmX;
disY = 48*3.141592*rpmY;
RtpwmX = Rt_X.SetPV(disX , goalX);
RtpwmY = Rt_Y.SetPV(disY , goalY);
if(controller->Button.B){
Rt_flagX = true;
}
Rt_flagY = true;
if (Rt_flagY){
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);
}
if(goalY - 15 < disY && disY < goalY + 15){
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
Rt_flagY = false;
Rt_flagX = true;
}
if(Rt_flagX){
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);
}
if(goalX - 15 < disX && disX < goalX + 15){
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
Rt_flagX = false;
}else{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
}
pc.printf("%f \r\n",RtpwmX);
wait_ms(50);
/*if(LimitSw::IsPressed(9)) LED_DEBUG0 = LED_ON;
else LED_DEBUG0 = LED_OFF;*/
#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 = 1;
BuzzerTimer.attach(BuzzerTimer_func, 1.2);
Emsflag = true;
}
if(!EMS_0 && !EMS_1) {
buzzer = 0;
BuzzerTimer.detach();
Emsflag = false;
}
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;
}
}
}
#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() {
buzzer = !buzzer;
}
#pragma endregion