File content as of revision 2:c015739085d3:

#include "mbed.h"
#include "Process.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;

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 TILE_FR 0 //足回り前右
#define TILE_FL 1 //足回り前左
#define TILE_BR 2 //足回り後右
#define TILE_BL 3 //足回り後左

#define Anguladjust_R 4 //角度調節右
#define Anguladjust_L 5 //角度調節左

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;

//************ライントレース変数*******************
	int Point[3] = {234, 466, 590};//赤，緑，青
	
	int startP = 150;
	int downP = 70;
	
	bool compA = false;
	bool compB = false;
	bool compC = false;
	bool compD = false;
	
	bool invationA = false;
	bool invationB = false;
	bool invationC = false;
	bool invationD = false;
//************ライントレース変数*******************

int averageR_0;
int averageG_0;
int averageB_0;
int averageR_1;
int averageG_1;
int averageB_1;
int averageR_2;
int averageG_2;
int averageB_2;
int averageR_3;
int averageG_3;
int averageB_3;

void ColorDetection();

#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.*/

	#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)
	{        	
	   	float x = 0, y= 0, z = 0;
	   	
	    pc.printf("X:%1.3f , Y:%1.3f , Z:%1.3f \r\n",acc[0].read(),acc[1].read(),acc[2].read());
	    
	    x = acc[0]*1000;
	    y = acc[1]*1000;
	    z = acc[2]*1000;
	    
	    pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n",x,y,z);
	    
	    float rotateX = (x - 306)/2.22 - 90;
	    float rotateY = (y - 305)/2.21 - 90;
	    pc.printf("X:%3.1f , Y:%3.1f \r\n" , rotateX , rotateY);
		wait_ms(50);

		#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
			{
				//ロック時の処理
			}
		}
		
		SystemProcessUpdate();
	}
}


	

#pragma region PROCESS
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0()
{
	ColorDetection();
}
#endif

#if USE_PROCESS_NUM>1
static void Process1()
{
    motor[0].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]) * 0.8;
    motor[1].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]) * 0.8;
    motor[2].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]) * 0.8; 
    motor[3].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]) * 0.8;
         
    motor[0].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]);
   	motor[1].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]);
    motor[2].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]);
    motor[3].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]);
                  
    if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
    motor[0].pwm = motor[0].pwm * 1.3;
    motor[1].pwm = motor[1].pwm * 1.3;
    
    }
}
#endif

bool buttoncomp = false;
#if USE_PROCESS_NUM>2
static void Process2()
{
	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(controller->Button.A && buttoncomp = false)
	{
		motor[0].dir = dir;
		motor[0].pwm = startP;
	}
	
	if(invationA)
	{
	   motor[0].PWM = startP
		
	}
	
	
	
	
}
#endif

#if USE_PROCESS_NUM>3
static void Process3()
{
	if(controller->Button.R){
	    motor[4].dir = FOR;
	    motor[5].dir = BACK;
	    motor[4].pwm = 150;
	    motor[5].pwm = 150;
	}else if(controller->Button.L){
	    motor[4].dir = BACK;
	    motor[5].dir = FOR;
	    motor[4].pwm = 150;
	    motor[5].pwm = 150;
	}else{
	    motor[4].dir = BRAKE;
	    motor[5].dir = BRAKE;
	}  
	if(LimitSw::IsPressed(0)){
	    motor[4].dir = BRAKE;
	    motor[5].dir = BRAKE;
	}else if(LimitSw::IsPressed(1)){
	    motor[4].dir = BRAKE;
	    motor[5].dir = BRAKE;
	} 
}
#endif

#if USE_PROCESS_NUM>4
static void Process4()
{
	//ColorDetection();
	
    for(int i=0;i<=10;i++)
    {
    	ColorDetection();
    	
    	averageR_0 += Color_A[0];
    	averageG_0 += Color_A[1];
    	averageB_0 += Color_A[2];
    	averageR_1 += Color_B[0];
    	averageG_1 += Color_B[1];
    	averageB_1 += Color_B[2];
    	averageR_2 += Color_C[0];
    	averageG_2 += Color_C[1];
    	averageB_2 += Color_C[2];
    	averageR_3 += Color_D[0];
    	averageG_3 += Color_D[1];
    	averageB_3 += Color_D[2];
    }
    pc.printf("AR_0:%d, AG_0:%d ,AB_0:%d \r\n",averageR_0 / 10 ,averageG_0 / 10, averageB_0 / 10);
    pc.printf("AR_1:%d, AG_1:%d ,AB_1:%d \r\n",averageR_1 / 10 ,averageG_1 / 10, averageB_1 / 10);
    pc.printf("AR_2:%d, AG_2:%d ,AB_2:%d \r\n",averageR_2 / 10 ,averageG_2 / 10, averageB_2 / 10);
    pc.printf("AR_3:%d, AG_3:%d ,AB_3:%d \r\n",averageR_3 / 10 ,averageG_3 / 10, averageB_3 / 10);
	
	averageR_0 = 0;
    averageG_0 = 0;
	averageB_0 = 0;
	averageR_1 = 0;
    averageG_1 = 0;
	averageB_1 = 0;
	averageR_2 = 0;
    averageG_2 = 0;
	averageB_2 = 0;
	averageR_3 = 0;
    averageG_3 = 0;
	averageB_3 = 0;
}
#endif

#if USE_PROCESS_NUM>5
static void Process5()
{
	pc.printf("X:1.3% , Y:1.3%f , Z:1.3%f \r\n",acc[0].read(),acc[1].read(),acc[2].read());
	//int rotateX = (acc[0].read()-)/ -90;
	//int rotateY = (acc[1].read()-)/ -90;
	//pc.printf("X:%d ,Y:%d", rotateX, rotateY);
	wait_ms(50);
}
#endif

#if USE_PROCESS_NUM>6
static void Process6()
{

}
#endif

#if USE_PROCESS_NUM>7
static void Process7()
{

}
#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 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); //緑
        
    //pc.printf("Red_0=%d , Green_0=%d , Blue_0=%d",Color_A[0],Color_A[1],Color_A[2]);
    //pc.printf("\r\n");
        
    Color_B[0] = ColorIn(1);
    wait_us(3);
    Color_B[1] = ColorIn(1);
    wait_us(3);
    Color_B[2] = ColorIn(1);
        
    //pc.printf("Red_1=%d , Green_1=%d , Blue_1=%d",Color_B[0],Color_B[1],Color_B[2]);
    //pc.printf("\r\n");
         
    Color_C[0] = ColorIn(2);
    wait_us(3);
    Color_C[1] = ColorIn(2);
    wait_us(3);
    Color_C[2] = ColorIn(2);
        
    /*pc.printf("Red_2=%d , Green_2=%d , Blue_2=%d",Color_C[0],Color_C[1],Color_C[2]);
    pc.printf("\r\n");*/
         
    Color_D[0] = ColorIn(3);
    wait_us(3);
    Color_D[1] = ColorIn(3);
    wait_us(3);
    Color_D[2] = ColorIn(3);
        
    /*pc.printf("Red_3=%d , Green_3=%d , Blue_3=%d",Color_D[0],Color_D[1],Color_D[2]);
    pc.printf("\r\n");*/
}
#pragma endregion