Exemplo n.º 1
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);
      }
    }
  }
}
Exemplo n.º 2
0
  void Measure(void) {
    int us, cm;
    int buf[8];

    us = US_Measure_us();
    cm = US_usToCentimeters(us, 22);
    if(cm < 10){

	   //LED_SetVal();

    }
  }
Exemplo n.º 3
0
static void US_PrintStatus(const CLS1_StdIOType *io) {
  uint8_t buf[16];
  uint16_t cm;
  
  cm = US_usToCentimeters(usDevice.lastValue_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);
}
Exemplo n.º 4
0
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;
}
Exemplo n.º 5
0
uint16_t US_GetLastCentimeterValue(void) {
  return US_usToCentimeters(usDevice.lastValue_us, 22);
}
Exemplo n.º 6
0
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 */
}
Exemplo n.º 7
0
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 */
	}