void GPS_TO_MASTER(void)
{
MISO_GPS_X.f64_data = Gps_List[0].position.x;
Write_Database(S_GPS_X_ID);
MISO_GPS_Y.f64_data = Gps_List[0].position.y;
Write_Database(S_GPS_Y_ID);
MISO_GPS_ANG.f64_data = Gps_List[0].radian;
Write_Database(S_GPS_ANG_ID);
}
void W_MOTOR_SPEED_ANGLE_FUNC(ID_NUMDEF ID_NUM , union fi32_to_u8 SPEED_ANGLE)
{
uint8_t i;
for(i=0;i<4;i++)
{
*(Can_Database[HASH_TABLE[ID_NUM]].Data_ptr+i) = SPEED_ANGLE.u8_data[i];
}
Write_Database(ID_NUM);
}
void W_PWM_FUNC(double pwm0, double pwm1, double pwm2, double pwm3)
{
union fi64_to_u8 temp;
temp.u16_data[0] = (uint16_t)(pwm0*10);
temp.u16_data[1] = (uint16_t)(pwm1*10);
temp.u16_data[2] = (uint16_t)(pwm2*10);
temp.u16_data[3] = (uint16_t)(pwm3*10);
MOSI_PWM.f64_data = temp.f64_data;
Write_Database(W_PWM_ID);
}
void W_STEPPER_FUNC(u8 dir,u8 free,u32 step)
{
union u32_to_u8{
u32 u32_data;
u8 u8_data[4];
}stepper;
MOSI_stepper.u8_data[0]=dir;
MOSI_stepper.u8_data[1]=free;
stepper.u32_data=step;
MOSI_stepper.u8_data[2]=stepper.u8_data[0];
MOSI_stepper.u8_data[3]=stepper.u8_data[1];
MOSI_stepper.u8_data[4]=stepper.u8_data[2];
MOSI_stepper.u8_data[5]=stepper.u8_data[3];
Write_Database(W_STEPPER_ID);
}
void W_MOTOR_OLD_FUNC(ID_NUMDEF ID_NUM, float rot, int16_t speed,int8_t cmd)
{
union uPosType
{
uint8_t d[4]; /* data */
float v; /* value */
}uRot; /* union position */
union uSpeedType
{
uint8_t d[2]; /* data */
int16_t v; /* value */
}uSpeed; /* union speed */
if(ID_NUM>W_MOTOR8_OLD_ID || ID_NUM<W_MOTOR1_OLD_ID)
{
/* add code here to report error */
return;
}
uRot.v = rot ;
uSpeed.v = speed ;
/* send goal position data by can bus */
*(Can_Database[HASH_TABLE[ID_NUM]].Data_ptr+0) = uRot.d[0];
*(Can_Database[HASH_TABLE[ID_NUM]].Data_ptr+1) = uRot.d[1];
*(Can_Database[HASH_TABLE[ID_NUM]].Data_ptr+2) = uRot.d[2];
*(Can_Database[HASH_TABLE[ID_NUM]].Data_ptr+3) = uRot.d[3];
*(Can_Database[HASH_TABLE[ID_NUM]].Data_ptr+4) = uSpeed.d[0];
*(Can_Database[HASH_TABLE[ID_NUM]].Data_ptr+5) = uSpeed.d[1];
*(Can_Database[HASH_TABLE[ID_NUM]].Data_ptr+6) = CURRENT_ROBOT;
*(Can_Database[HASH_TABLE[ID_NUM]].Data_ptr+7) = cmd;
Write_Database(ID_NUM);
}
void S_ENC1_C1_FUNC(void)
{
MISO_ENC1_C1.f64_data = Encoders[0].Convert1;
Write_Database(S_ENC1_C1_ID);
}
void S_GYRO_RAD_FUNC(void)
{
MISO_GYRO_RAD.f64_data = Gyro_Angle_Total;
Write_Database(S_GYRO_RAD_ID);
}
void S_GYRO_C2_FUNC(void)
{
MISO_GYRO_C2.f64_data = Gyro_Convert2;
Write_Database(S_GYRO_C2_ID);
}
void S_GYRO_C1_FUNC(void)
{
MISO_GYRO_C1.f64_data = Gyro_Convert1;
Write_Database(S_GYRO_C1_ID);
}
void S_GPS_ANG_FUNC(void)
{
MISO_GPS_ANG.f64_data = Gps_List[0].radian;
Write_Database(S_GPS_ANG_ID);
}
void S_ENC2_ANG_FUNC(void)
{
MISO_ENC2_ANG.f64_data = Encoders[1].radian;
Write_Database(S_ENC2_ANG_ID);
}
void W_MOTOR_ANGLE_FUNC(void)
{
MOSI_MOTOR1_ANGLE.i32_data=1;
Write_Database(W_MOTOR1_ANGLE_ID);
}
void CheckFloat(void)
{
MISO_CMD = NO_COMMAND;
MOSI_CMD = CHECK_FLOAT;
Write_Database(M_G_CMD_ID);
}
void W_FLAG_FUNC(void)
{
FLAG_SET[0]=0x01;
// FLAG_SET[1]=0x00;
Write_Database(W_FLAG_SET_ID);
}
void S_ENC2_C2_FUNC(void)
{
MISO_ENC2_C2.f64_data = Encoders[1].Convert2;
Write_Database(S_ENC2_C2_ID);}
void S_ENC2_RAD_FUNC(void)
{
MISO_ENC2_RAD.f64_data = Encoders[1].Radius;
Write_Database(S_ENC2_RAD_ID);
}
void S_GPS_X_FUNC(void)
{
MISO_GPS_X.f64_data = Gps_List[0].position.x;
Write_Database(S_GPS_X_ID);
}
void S_ENC2_DIS_FUNC(void)
{
MISO_ENC2_DIS.f64_data = Encoders[1].Distance;
Write_Database(S_ENC2_DIS_ID);
}
void S_GPS_Y_FUNC(void)
{
MISO_GPS_Y.f64_data = Gps_List[0].position.y;
Write_Database(S_GPS_Y_ID);
}
