MB2019_main_11_12

Users » kishibekairohan » Code » MB2019_main_11_12
Taiki Maruyama / Mbed 2 deprecated MB2019_main_11_12
System/Process/Process.cpp

Committer:: M_souta
Date:: 2019-09-09
Revision:: 21:e3b58d675c1c
Parent:: 20:eae8c84f318c
Child:: 22:7d93f79a3686
File content as of revision 21:e3b58d675c1c:


#include "mbed.h"
#include "Process.h"
#include "QEI.h"

#include "../../CommonLibraries/PID/PID.h"
#include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
#include "../../Communication/RS485/LineHub/LineHub.h"
#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 "../../LED/LED.h"
#include "../../Safty/Safty.h"
#include "../Using.h"

using namespace SWITCH;
using namespace PID_SPACE;
using namespace ENCODER;
using namespace LINEHUB;

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);

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

//**************Buzzer****************
//DigitalOut buzzer(BUZZER_PIN);
void BuzzerTimer_func();
Ticker BuzzerTimer;
bool EMGflag = false;
PwmOut buzzer(BUZZER_PIN);
//**************Buzzer****************

//************TapeLed*****************
void TapeLedEms_func();
TapeLedData tapeLED;
TapeLedData sendLedData;
TapeLED_Mode ledMode = Normal;
Ticker tapeLedTimer;
//************TapaLed*****************

const int omni[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);
}

#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();
	//LINEHUB::LineHub::Update();
	#endif
	
}



void SystemProcess()
{
	SystemProcessInitialize();

	while(1)
	{
		int g[8];
		for(int i = 0; i < 8; i++){
			g[i] = 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]);
		
		buzzer.period(1.0/800);
		
		#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
			{
				//ロック時の処理
			}
		}
		
		//Emergency!
		if(!EMG_0 && !EMG_1 && !EMGflag){
			buzzer = 0;
			BuzzerTimer.attach(BuzzerTimer_func, 1);
			EMGflag = true;
			LED_DEBUG0 = 1;
		}
		if(EMG_0 && EMG_1 && EMGflag){
			buzzer = 1;
			BuzzerTimer.detach();
			EMGflag = false;
		}
		SystemProcessUpdate();
	}
}


	

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

#if USE_PROCESS_NUM>1
static void Process1()
{
    /*
    if(controller->Button.UP) {
    	motor[LIFT_LB].dir = FOR;
    	motor[LIFT_LB].pwm = 180;
    	motor[LIFT_RB].dir = BACK;
    	motor[LIFT_RB].pwm = 180;
    } else if(controller->Button.DOWN) {
    	motor[LIFT_LB].dir = BACK;
    	motor[LIFT_LB].pwm = 180;
    	motor[LIFT_RB].dir = FOR;
    	motor[LIFT_RB].pwm = 180;
    } else {
    	motor[LIFT_LB].dir = BRAKE;
    	motor[LIFT_LB].pwm = 200;
    	motor[LIFT_RB].dir = BRAKE;
    	motor[LIFT_RB].pwm = 200;
    }
    */
    
    if(controller->Button.X) {
    	motor[LIFT_U].dir = FOR;
    	motor[LIFT_U].pwm = 180;
    } else if(controller->Button.Y) {
    	motor[LIFT_U].dir = BACK;
    	motor[LIFT_U].pwm = 180;
    } else {
    	motor[LIFT_U].dir = BRAKE;
    	motor[LIFT_U].pwm = 180;
    }

	if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
	    motor[TIRE_BL].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X]     );
		motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]         );
		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])   ;	
	} else {
		motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
		motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
		motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
		motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
	   
		motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
		motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
		motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
		motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
	}
}
#endif

#if USE_PROCESS_NUM>2
static void Process2()
{	
	
}
#endif

#if USE_PROCESS_NUM>3
static void Process3() 
{
	
}
#endif

#if USE_PROCESS_NUM>4
static void Process4() 
{	

}
#endif

#if USE_PROCESS_NUM>5
static void Process5() 
{	
	
}
#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
}

void BuzzerTimer_func(){
    buzzer = !buzzer;
    //LED_DEBUG0 = !LED_DEBUG0;
}

void TapeLedEms_func() {
    sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
}

#pragma region USER-DEFINED-FUNCTIONS


#pragma endregion
Repository toolbox

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	12 Nov 2019
Imports:	1
Forks:	0
Commits:	48
Dependents:	0
Dependencies:	1
Followers:	1
Important changes to repositories hosted on mbed.com

System/Process/Process.cpp

File content as of revision 21:e3b58d675c1c:

Repository toolbox

Repository details

Important Information for this Arm website

Important changes to repositories hosted on mbed.com

System/Process/Process.cpp

File content as of revision 21:e3b58d675c1c:

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
