void ACCEL_GetValues(int16_t *x, int16_t *y, int16_t *z) {
int16_t xmg, ymg, zmg;
xmg = MMA1_GetXmg();
ymg = MMA1_GetYmg();
zmg = MMA1_GetZmg();
*x = xmg;
*y = ymg;
*z = zmg;
}
static portTASK_FUNCTION(Remote, pvParameters) {
int32_t valMotR = 0;
int32_t valMotL = 0;
for(;;) {
#if PL_HAS_REMOTE && PL_IS_FRDM
protocol42 txdata;
if (analogOn) {
GetXY(&x_anal,&y_anal);
txdata.target = isROBOcop;
txdata.type = anal_x;
txdata.data = x_anal;
sendData42(txdata);
txdata.type = anal_y;
txdata.data = y_anal;
sendData42(txdata);
}
if (accelOn){
x_ac = MMA1_GetXmg();
y_ac = MMA1_GetYmg();
x_accel = scaleFromAccelToU8(x_ac);
y_accel = scaleFromAccelToU8(y_ac);
txdata.target = isROBOcop;
txdata.type = accel_x;
txdata.data = x_accel;
sendData42(txdata);
txdata.type = accel_y;
txdata.data = y_accel;
sendData42(txdata);
}
#endif
#if PL_HAS_MOTOR
if(valX > 0) {
valMotR = valY-valX;
valMotL = valY;
}
else if (valX < 0) {
valMotR = valY;
valMotL = valY+valX;
}
else {
valMotR = valY;
valMotL = valY;
}
if((valY < 3) && (valY > -3)) {
valMotR = 0;
valMotL = 0;
}
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), valMotR);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT) , valMotL);
#endif
FRTOS1_vTaskDelay(20/TRG_TICKS_MS);
}
}
static void ACCEL_GetValues(int16_t *x, int16_t *y, int16_t *z) {
*x = MMA1_GetXmg();
*y = MMA1_GetYmg();
*z = MMA1_GetZmg();
}
static void StateMachine_Run(void){
int i;
int distance;
// Für Acceleration Sensor
int16_t x = MMA1_GetXmg();
WAIT1_Waitms(5);
int16_t y = MMA1_GetYmg();
switch (state) {
case START:
//SHELL_SendString((unsigned char*)"INFO: No calibration data present.\r\n");
WAIT1_Waitms(4700);
DRV_EnableDisable(1);
DRV_SetSpeed(3000,3000);
//WAIT1_Waitms(40);
//DRV_SetSpeed(3000,3000);
//state = ATTACK;
//DRV_SetSpeed(-4200,4200);
// MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), MOT_DIR_FORWARD);
//MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), MOT_DIR_FORWARD);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 30);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 30);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 40);
/*int x0;
int x5;
int x0_cal;
int x5_cal;
x0 = Get_Reflectance_Values(0);
x0_cal = x0 / 15;
x5 = Get_Reflectance_Values(5);
x5_cal = x5 / 15;
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), x0_cal);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), x5_cal);
*/
//uint16_t* calib_values_pointer;
//calib_values_pointer = &SensorTimeType;
//calib_values_pointer = S1_GetVal();
state = DRIVE;
break;
case DRIVE:
DRV_SetSpeed(-4200,4200);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 40);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 40);
if((Get_Reflectance_Values(0) <= 100) || (Get_Reflectance_Values(5) <= 100)) // 0 = weiss / 1000 = schwarz
{
state = TURN;
break;
}
distance = US_usToCentimeters(US_Measure_us(),22);
if((distance <= 30) && (distance != 0)){
state = ATTACK;
}
/*
if((x >= 2000) || (x <= 2000))
{
state = STOP;
}
if((y >= 2000) || (y <= 2000))
{
state = STOP;
}
*/
break;
case TURN:
//SHELL_SendString((unsigned char*)"...stopping calibration.\r\n");
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -40);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -40);
// ATTACK
DRV_SetSpeed(-5000,-5000);
WAIT1_Waitms(500);
/*
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -40);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 40);
*/
DRV_SetSpeed(4200,-4200);
WAIT1_Waitms(300);
DRV_SetSpeed(5000,5000);
WAIT1_Waitms(50);
if((Get_Reflectance_Values(0) > 400) || (Get_Reflectance_Values(5) > 500))
{
state = DRIVE;
}
break;
case ATTACK:
DRV_SetSpeed(5500,5500);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 80);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 80);
if((Get_Reflectance_Values(0) <= 100) || (Get_Reflectance_Values(5) <= 100)) // 0 = weiss / 1000 = schwarz
{
state = TURN;
}
/*
if((x >= 2000) || (x <= -2000))
{
state = STOP;
}
if((y>= 2000) || (y <= -2000))
{
state = STOP;
}
*/
break;
case STOP:
DRV_SetSpeed(0,0);
/*MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);*/
/*
if((x >= -300) && (x <= 300))
{
if((y >= -300) && (y <= 300))
{
state = DRIVE;
}
}
*/
break;
} /* whitch */
}
