void PosixProcess::ForkAndExec()
{
nativeIn->CreateHandles();
nativeOut->CreateHandles();
nativeErr->CreateHandles();
int pid = fork();
if (pid < 0)
{
throw ValueException::FromFormat("Cannot fork process for %s",
args->At(0)->ToString());
}
else if (pid == 0)
{
// outPipe and errPipe may be the same, so we dup first and close later
dup2(nativeIn->GetReadHandle(), STDIN_FILENO);
dup2(nativeOut->GetWriteHandle(), STDOUT_FILENO);
dup2(nativeErr->GetWriteHandle(), STDERR_FILENO);
nativeIn->CloseNative();
nativeOut->CloseNative();
nativeErr->CloseNative();
// close all open file descriptors other than stdin, stdout, stderr
for (int i = 3; i < getdtablesize(); ++i)
close(i);
size_t i = 0;
char** argv = new char*[args->Size() + 1];
//argv[i++] = const_cast<char*>(command.c_str());
for (;i < args->Size(); i++)
{
argv[i] = const_cast<char*>(args->At(i)->ToString());
}
argv[i] = NULL;
SharedStringList envNames = environment->GetPropertyNames();
for (i = 0; i < envNames->size(); i++)
{
const char* key = envNames->at(i)->c_str();
std::string value(environment->Get(key)->ToString());
setenv(key, value.c_str(), 1);
}
const char *command = args->At(0)->ToString();
execvp(command, argv);
_exit(72);
}
SetPID(pid);
nativeIn->CloseNativeRead();
nativeOut->CloseNativeWrite();
nativeErr->CloseNativeWrite();
}
void HorizontalAlign::Execute()
{
//Only if we need to FIND a target
if (!hasTarget)
{
hasTarget = Robot::vision->UpdateCurrentTarget();
}
else
{
if (!GetPIDController()->IsEnabled())
{
PIDUserDisabled = false;
//GetPIDController()->SetPID(1, 0, 0);
auto pid = GetPIDController();
pid->SetPID(SmartDashboard::GetNumber("dP", 0.004), SmartDashboard::GetNumber("dI", 0), SmartDashboard::GetNumber("dD", 0));
pid->Enable();
}
else
printf("PID Enabled\n");
}
}
void PIDController::ValueChanged(ITable* source, llvm::StringRef key,
std::shared_ptr<nt::Value> value, bool isNew) {
if (key == kP || key == kI || key == kD || key == kF) {
if (m_P != m_table->GetNumber(kP, 0.0) ||
m_I != m_table->GetNumber(kI, 0.0) ||
m_D != m_table->GetNumber(kD, 0.0) ||
m_F != m_table->GetNumber(kF, 0.0)) {
SetPID(m_table->GetNumber(kP, 0.0), m_table->GetNumber(kI, 0.0),
m_table->GetNumber(kD, 0.0), m_table->GetNumber(kF, 0.0));
}
} else if (key == kSetpoint && value->IsDouble() &&
m_setpoint != value->GetDouble()) {
SetSetpoint(value->GetDouble());
} else if (key == kEnabled && value->IsBoolean() &&
m_enabled != value->GetBoolean()) {
if (value->GetBoolean()) {
Enable();
} else {
Disable();
}
}
}
void CommandProcess()
{
// Read incoming control messages
if (Serial_available() >= 2)
{
char start=Serial_read();
if (start == '@') {// Start of new control message
int command = Serial_read(); // Commands
if (command == 'h') {//Hook AHRS Stack Device
// Read ID
char id[2];
id[0] = GetChar();
id[1] = GetChar();
// Reply with synch message
printf("@HOOK");
Serial_write(id, 2);
}
else if (command == 'v') {//Check Version
CheckVersion();
}
else if (command == 'c') {// A 'c'calibration command
SensorCalibration();
}
else if (command == 'b') {// 'b'lock erase flash
FlashControl();
}
else if (command == 'p') {// Set 'p'id command
SetPID();
}
else if (command == 'm') {// Set report 'm'ode
char mode = GetChar();
if (mode == 'e') {// Report AHRS by 'e'uler angle
report_mode = REPORT_AHRS_EULER;
}
else if (mode == 'q') {// Report // Report AHRS by 'q'quaternion
report_mode = REPORT_AHRS_QUATERNION;
}
else if (mode == 'r') {// Report sensor 'r'aw data
report_mode = REPORT_SENSORS_RAW;
}
else if (mode == 'c') {// Report sensor 'c'alibrated data
report_mode = REPORT_SENSORS_CALIBRATED;
}
else if (mode == 'm') {// Report 'm'otor power distribution
report_mode = REPORT_MOTOR_POWER;
}
else if (mode == 'v') {// Report 'v'elocity
report_mode = REPORT_VELOCITY;
}
else if (mode == 's') {// Report 's'tatus
report_status();
}
else if (mode == 'p') {// Report controller 'p'id
char type = GetChar();
if (type == 'p') // 'p'id
report_mode = REPORT_PID;
else if (type == 'r') //'r'ate pid
report_mode = REPORT_RATE_PID;
else if (type == 'a') //'a'ltitude hold pid
report_mode = REPORT_ALTHOLD_PID;
}
}
else if (command == 'f') {// Set report 'f'ormat
char format = GetChar();
if (format == 'b') {// Report 'b'inary format
report_format = REPORT_FORMAT_BINARY;
}
else if (format == 't') {// Report 't'ext format
report_format = REPORT_FORMAT_TEXT;
}
}
else if (command == 's') {// 's'tream output control
char mode = GetChar();
if (mode == 's') {// 's'tart stream
stream_mode = STREAM_START;
}
else if (mode == 'p') {// 'p'ause stream
stream_mode = STREAM_PAUSE;
}
else if (mode == 't') {// 't'oggle stream
if(stream_mode==STREAM_START)
stream_mode = STREAM_PAUSE;
else
stream_mode = STREAM_START;
}
}
}
else {
if (report_format == REPORT_FORMAT_TEXT) {
printf("Unknown command.\n");
}
} // Skip character
}
}
void SmartMotor::SetPID(int mode, double P, double I, double D){
SetPID(P,I,D);
SetMode(mode);
}
int main(void)
{
u8 i;
u8 RecvBuf[32];
u8 SendBuf[32];
u8 offline = 0;
u8 recv_flag = 0;
u32 pd2ms=0,pd20ams=0,pd20bms=0,pd100ms=0;
SystemInit();
RCC_Configuration();
NVIC_Configuration();
GPIO_Configuration();
USART1_Configuration();
dbgPrintf(" Init Ticktack !\r\n");
cycleCounterInit();
SysTick_Config(SystemCoreClock / 1000);
for(i=0;i<2;i++)
{
OP_LED1;OP_LED2;OP_LED3;OP_LED4;
delay_ms(500);
OP_LED1;OP_LED2;OP_LED3;OP_LED4;
delay_ms(500);
}
FilterInit();
controllerInit();
dbgPrintf(" Init eeprom!\r\n");
FLASH_Unlock();
EE_Init();
EE_Read_ACC_GYRO_Offset();
/* 如果PID丢失或者错误，将下面两行注释去掉，重新编译烧写，运行一遍，可将PID还原，然后重新注释，再烧写一遍 */
//EE_Write_PID();
//EE_Write_Rate_PID();
EE_Read_PID();
EE_Read_Rate_PID();
dbgPrintf(" Init adc!\r\n");
ADC_DMA_Init();
dbgPrintf(" Init NRF24L01 !\r\n");
SPI_NRF_Init();
Nrf24l01_Init();
NRF24l01_SetRxMode();
while(Nrf24l01_Check())
{
dbgPrintf("NRF24L01 Fault !\r\n");
delay_ms(500);
}
dbgPrintf("NRF24L01 Is Detected !\r\n");
dbgPrintf("Init MPU6050...\r\n");
IIC_Init();
MPU6050_initialize();
dbgPrintf("Init Motor...\r\n");
Motor_Init();
Motor_SetPwm(0,0,0,0);
pd20bms = TIMIRQCNT + 10*ITS_PER_MS;
while(1)
{
if(TIMIRQCNT>pd2ms + 2*ITS_PER_MS-1) // every 4ms
{
GetEulerAngle();
if(lock_flag==UNLOCK)
AttitudeToMotors(angle.y,angle.x,angle.z);
else
{
MOTOR1=0;
MOTOR2=0;
MOTOR3=0;
MOTOR4=0;
}
Motor_SetPwm(MOTOR1,MOTOR2,MOTOR3,MOTOR4);
pd2ms = TIMIRQCNT;
}
if(TIMIRQCNT>pd20ams + 20*ITS_PER_MS-1) // every 20ms
{
if(NRF24l01_Recv(RecvBuf)>10)
{
if((RecvBuf[RecvBuf[2]+3]==CheckSum(RecvBuf, RecvBuf[2]+3))&&(RecvBuf[0]==0xAA))
{
if(RecvBuf[1]!=0xC0)
OP_LED1;
offline=0;
switch(RecvBuf[1])
{
case 0xC0: //control
Getdesireddata(RecvBuf);
OP_LED2;
break;
case 0x10: //W PID
SetPID(RecvBuf);
break;
case 0x11: //W Attitude
SetAccGyroOffset(RecvBuf);
break;
case 0x12: //W Control offset
break;
case 0x14: //W Rate PID
SetRatePID(RecvBuf);
break;
case 0x20: //R PID
recv_flag = RESEND;
GetPID(SendBuf);
break;
case 0x21: //R Attitude
recv_flag = RESEND;
GetAccGyroOffset(SendBuf);
break;
case 0x22: //R Control offset
break;
case 0x24: //R Rate PID
recv_flag = RESEND;
GetRatePID(SendBuf);
break;
case 0x40: //校准姿态
EnableCalibration();
break;
case 0x41: //校准遥控器零点
break;
default:
break;
}
}
}
pd20ams = TIMIRQCNT;
}
if(TIMIRQCNT>pd20bms + 20*ITS_PER_MS-1) // every 20ms
{
if(recv_flag==0)
GetState(SendBuf);
else
recv_flag--;
NRF_SendData(SendBuf);
OP_LED3;
pd20bms = TIMIRQCNT;
}
if(TIMIRQCNT>pd100ms + 100*ITS_PER_MS-1) // every 100ms
{
if(offline>20)
lock_flag = LOCK;
offline++;
// OP_LED4;
pd100ms = TIMIRQCNT;
}
}
}
