static portTASK_FUNCTION(MainTask, pvParameters) { uint16_t msCnt; (void) pvParameters; /* parameter not used */ #if PL_HAS_ACCEL ACCEL_LowLevelInit(); #endif for (;;) { EVNT_HandleEvent(APP_EvntHandler); #if PL_HAS_KEYS && !PL_HAS_KBI KEY_Scan(); /* poll keys */ #endif FRTOS1_vTaskDelay(20/portTICK_RATE_MS); msCnt += 20; if (msCnt > 280) { #if PL_HAS_ULTRASONIC (void)US_Measure_us(); /* Measure distance */ #endif msCnt = 0; } } }
static void FollowObstacle(void) { #define DUTY_SLOW 16 #define DUTY_MEDIUM 20 #define DUTY_FAST 23 static uint8_t cnt; uint16_t cm, us; cnt++; /* get called with 100 Hz, reduce to 10 Hz */ if (cnt==10) { us = US_Measure_us(); cnt = 0; if (followObstacle) { cm = US_usToCentimeters(us, 22); if (cm<10) { /* back up! */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -DUTY_SLOW); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -DUTY_SLOW); } else if (cm>=10 && cm<=20) { /* stand still */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); TURN_Turn(TURN_RIGHT45); /* try to avoid obstacle */ } else if (cm>20 && cm<=40) { /* forward slowly */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), DUTY_MEDIUM); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), DUTY_MEDIUM); } else if (cm>40 && cm<100) { /* forward fast */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), DUTY_FAST); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), DUTY_FAST); } else { /* nothing in range */ MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); } } } }
void Measure(void) { int us, cm; int buf[8]; us = US_Measure_us(); cm = US_usToCentimeters(us, 22); if(cm < 10){ //LED_SetVal(); } }
static void US_PrintStatus(const CLS1_StdIOType *io) { uint8_t buf[16]; uint16_t cm, us; us = US_Measure_us(); cm = US_usToCentimeters(us, 22); CLS1_SendStatusStr((unsigned char*)"ultrasonic", (unsigned char*)"\r\n", io->stdOut); UTIL1_Num16uToStr(buf, sizeof(buf), usDevice.lastValue_us); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n"); CLS1_SendStatusStr((unsigned char*)" us", buf, io->stdOut); UTIL1_Num16uToStr(buf, sizeof(buf), cm); UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n"); CLS1_SendStatusStr((unsigned char*)" cm", buf, io->stdOut); }
static uint8_t MeasureCm(void) { uint16_t us, cm; uint8_t buf[8]; us = US_Measure_us(); UTIL1_Num16uToStrFormatted(buf, sizeof(buf), us, ' ', 5); //LCD1_GotoXY(1,5); //LCD1_WriteString((char*)buf); cm = US_usToCentimeters(us, 22); UTIL1_Num16uToStrFormatted(buf, sizeof(buf), cm, ' ', 5); //LCD1_GotoXY(2,5); //LCD1_WriteString((char*)buf); LEDR_Put(cm>0 && cm<10); /* red LED if object closer than 10 cm */ // LEDB_Put(cm>=10&&cm<=100); /* blue LED if object is in 10..100 cm range */ LEDG_Put(cm>=10); /* blue LED if object is in 10..100 cm range */ return cm; }
/**\fn US_Main(void) * \brief Main task loop for US sensors * * Main task loop for Ultrasonic sensors */ void US_Main(void) { uint16_t microseconds, distance, tMessage = 0; US_Init(); /* delay for 1/4 second to allow other sensors to init */ FRTOS1_vTaskDelay(250/portTICK_RATE_MS); US_Message message; for(;;) { /* check for a temperature message and update temperature */ if(xQueueReceive(musQueueHandle, &tMessage, (portTickType)1)) { temperature = tMessage; } /* Loop through each sensor, measure, and send off to the queue */ for (usCounter = 0; usCounter < 4; usCounter++) { usDevices.currentDevice = usCounter; microseconds = US_Measure_us(usCounter); if (microseconds > 0) { distance = US_usToMM(microseconds, temperature); } else { distance = 0; /* overflowed */ } message.sensor = usCounter; message.distance = distance; xQueueSend(usQueueHandle, (void*)&message, (portTickType)5); /*send the message, block up to 5 ticks if queue isn't free */ FRTOS1_vTaskDelay(10/portTICK_RATE_MS); } } }
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 */ }