static void StateMachine_Run(void){ int i; int distance; switch (state) { case START: WAIT1_WaitOSms(800); BUZ_Beep(400,400); WAIT1_WaitOSms(800); BUZ_Beep(450,400); WAIT1_WaitOSms(800); BUZ_Beep(500,400); WAIT1_WaitOSms(800); BUZ_Beep(600,400); WAIT1_WaitOSms(800); BUZ_Beep(880,400); 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), 40); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 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: MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), SPEED_TURN); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -SPEED_TURN); if((Get_Reflectance_Values(3) <= 100) || (Get_Reflectance_Values(4) <= 100)){ // 0 = weiss / 1000 = schwarz state = STOP; } distance = US_usToCentimeters(US_Measure_us(),22); if((distance <= 30) && (distance != 0)){ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), SPEED_ATTACK); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), SPEED_ATTACK); state = ATTACK; } /*if((MMA1_GetXmg() >= 500) || (MMA1_GetXmg() <= -500)){ state = STOP; } */ /*if((MMA1_GetYmg() >= 500) || (MMA1_GetYmg() <= -500)){ state = STOP; }*/ break; case TURN: state = DRIVE; break; case STOP: MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -50); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -50); WAIT1_WaitOSms(150); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -50); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 50); WAIT1_WaitOSms(444); if(Get_Reflectance_Values(4) >= 400){ state = DRIVE; } /*if((MMA1_GetXmg() >= -200) && (MMA1_GetXmg() <= 200)){ state = DRIVE; }*/ /*if((MMA1_GetYmg() >= -200) || (MMA1_GetYmg() <= 200)){ state = DRIVE; }*/ break; case ATTACK: if((distance <= 30) && (distance != 0)){ state = ATTACK; MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), SPEED_ATTACK); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), SPEED_ATTACK); WAIT1_WaitOSms(50); }else{ state = DRIVE; } if((Get_Reflectance_Values(3) <= 100) || (Get_Reflectance_Values(4) <= 100)){ // 0 = weiss / 1000 = schwarz state = STOP; } break; } /* switch */ }
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 */ }