C++ (Cpp) Kierunek示例

示例#1

void reconstruct_reverse_track() {
	if (get_step_count() > 0) {
		TRACE_DEBUG("-- reconstruct_reverse_track -- \r\n");
		char txt_buff[16];
		sprintf(txt_buff, "Total steps: %2d", get_step_count());
		HY1602F6_Log("Reversing track", txt_buff);

		init_oa_configuration();
		CD4053_EnableIRSensors();
		waitms(1000);

		int step_count = get_step_count() - 1;
		for (; step_count >= 0; step_count--) {
			char lcd_buff1[16];
			sprintf(lcd_buff1, "angle: %3d", (int) steps_data[step_count]);
			char lcd_buff2[16];
			sprintf(lcd_buff2, "step: %3d", step_count);
			HY1602F6_Log(lcd_buff1, lcd_buff2);

			turn_of_angle(steps_data[step_count]);

			go_prosto();
		}

		Kierunek(RIGHT_ENGINES, STOP_GEAR);
		Kierunek(LEFT_ENGINES, STOP_GEAR);
	} else {
		TRACE_WARNING("-- No steps recorded. --\r\n");
		HY1602F6_Log("Invalid track", "No data found");
	}
}

示例#2

char go_prosto() {
	char result = 0;
	char step_count = 0;

	if (val == 2) {
		go_prosto_count++;
	}

	for (; step_count < 38; step_count++) {

		ADC_StartDoubleChannelConversion(ADC_CHANNEL_7, ADC_CHANNEL_5,
				onDistanceDataRdy);

		while (!ADC_IsDataReady())
			;

		OAA_OUTPUT oa_result = avoid_obstacles(level_mask);

		if (oa_result.gear_left == oa_result.gear_right) {
			PWM_Set(0, oa_result.speed_left);
			Kierunek(RIGHT_ENGINES, FORWARD_GEAR);
			Kierunek(LEFT_ENGINES, FORWARD_GEAR);
			waitms(100);
		}

		result = avoid_obstacle();
	}

	return result;
}

示例#3

void turn_at_magnetic_angle(double angle) {
	double turn_angle = 0;

	mag_info current_mg_i = MMC212xM_GetMagneticFieldInfo(&twid);
	double current_angle = compute_angle(current_mg_i.y, current_mg_i.x);
	turn_angle = compute_turn_angle(angle, current_angle);

	PWM_Set(0, 15);
	if (turn_angle > 0) {
		Kierunek(RIGHT_ENGINES, REVERSE_GEAR);
		Kierunek(LEFT_ENGINES, FORWARD_GEAR);
	} else {
		Kierunek(RIGHT_ENGINES, FORWARD_GEAR);
		Kierunek(LEFT_ENGINES, REVERSE_GEAR);
	}

	do {
		current_mg_i = MMC212xM_GetMagneticFieldInfo(&twid);
		current_angle = compute_angle(current_mg_i.y, current_mg_i.x);
		turn_angle = compute_turn_angle(angle, current_angle);

		waitms(5);

	} while (ABS(turn_angle) > 1);

	Kierunek(RIGHT_ENGINES, STOP_GEAR);
	Kierunek(LEFT_ENGINES, STOP_GEAR);

}

示例#4

void turn_of_angle(double angle) {
	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, 0x1 << 29);
	L3G4200D_Reset(&twid);

	PWM_Set(0, 15);
	waitms(1000);
	if (angle < 0) {
		TRACE_INFO("-- w lewo -- \r\n");
		Kierunek(RIGHT_ENGINES, FORWARD_GEAR);
		Kierunek(LEFT_ENGINES, REVERSE_GEAR);
	} else {
		TRACE_INFO("-- w prawo -- \r\n");
		Kierunek(RIGHT_ENGINES, REVERSE_GEAR);
		Kierunek(LEFT_ENGINES, FORWARD_GEAR);
	}

	while (ABS(angle) - ABS(L3G4200D_GetData().sAngle_z) > 0) {
		L3G4200D_ReadData(&twid);
	}

	L3G4200D_Reset(&twid);
	Kierunek(RIGHT_ENGINES, STOP_GEAR);
	Kierunek(LEFT_ENGINES, STOP_GEAR);

	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, 0x1 << 29);
}

示例#5

////////////////////////////////////////////////////////////////////////////////
// Proste sterowanie silnikami
// kierunek 0 - stop, 1 - w lewo, 2 - prosto, 3 - w prawo
////////////////////////////////////////////////////////////////////////////////
inline void go(char kierunek, char pwm1, char pwm2) {
	switch (kierunek) {
	case 0:
		Kierunek(1, 0);
		Kierunek(2, 0);
		PWM_Set(0, pwm1);
		PWM_Set(1, pwm1);
		PWM_Set(2, pwm2);
		PWM_Set(3, pwm2);
		break;

	case 1:
		Kierunek(1, 2);
		Kierunek(2, 1);
		PWM_Set(0, pwm1);
		PWM_Set(1, pwm1);
		PWM_Set(2, pwm2);
		PWM_Set(3, pwm2);
		break;

	case 2:
		Kierunek(1, 1);
		Kierunek(2, 1);
		PWM_Set(0, pwm1);
		PWM_Set(1, pwm1);
		PWM_Set(2, pwm2);
		PWM_Set(3, pwm2);
		break;

	case 3:
		Kierunek(1, 1);
		Kierunek(2, 2);
		PWM_Set(0, pwm1);
		PWM_Set(1, pwm1);
		PWM_Set(2, pwm2);
		PWM_Set(3, pwm2);
		break;
	}
}