Esempio n. 1
0
void forwardDistance(int distance, int left_speed, int right_speed, bool coast) 
{
	distanceLeft = distance;
	/*
	int curEnc = getLeftEncCount();
	turning = 0;
	while (getLeftEncCount() - curEnc < distance) {
		left_enc = getLeftEncCount();
		right_enc = getRightEncCount(); 
		//printf("Encoder counts left: %d\r\n",distance - getLeftEncCount() + curEnc); 
		//displayMatrix("FWD");
		targetLeft = left_speed;
		targetRight = right_speed;
	}

	if (!coast) {
		targetLeft = 0;
		targetRight = 0;
	}
	turning = 1;*/
	while (distanceLeft>=0) 
	{
		//targetSpeedX = 100;
		//targetSpeedW = 0;
		setLeftPwm(100);
		setRightPwm(100);
	}
		setLeftPwm(0);
		setRightPwm(0);
	//targetSpeedX = 0;
	//targetSpeedW = 0;
}
void lowBatCheck(void)
{
  if(voltage < 700) //alert when battery Voltage lower than 7V
	{	
		
		setLeftPwm(0);
		setRightPwm(0);
		
		while(1)
		{
			displayMatrix("Lbat");
			//beep_on;      
			ALL_LED_OFF;
			delay_ms(200);

			clearScreen();
			//beep_off;      
			ALL_LED_ON;
			delay_ms(200);			
		}
	}
  else {
		  displayInt(voltage);
		  delay_ms(1000);
	}		
}
Esempio n. 3
0
/* * * * * * * * * * * * * * * *
 *      Calculate Motor PWM    *
 *   Take speed variables and  *
 *  calculate motor pwm values *
 * * * * * * * * * * * * * * * */
void calculateMotorPwm()
{
	
 	int gyroFeedback;
 	int rotationalFeedback;
 	int sensorFeedback;
 	int leftBaseSpeed;
 	int rightBaseSpeed;
	//displayMatrix("1");//displayMatrix("PWM");
 	encoderFeedbackX = rightEncoderChange + leftEncoderChange;
 	encoderFeedbackW = rightEncoderChange - leftEncoderChange;
	//displayInt(encoderFeedbackX);
 	gyroFeedback = aSpeed/gyroFeedbackRatio; 
 	sensorFeedback = sensorError/a_scale; //what is a_scale?

 	if (onlyUseGyroFeedback)
 		rotationalFeedback = gyroFeedback;
 	else if (onlyUseEncoderFeedback)
 		rotationalFeedback = encoderFeedbackW;
 	else
 		rotationalFeedback = encoderFeedbackW + gyroFeedback;
	//printf("Encoder FeedbackX = %d\r\n", encoderFeedbackX);
 	posErrorX += curSpeedX - encoderFeedbackX;
 	posErrorW += curSpeedW - rotationalFeedback;
	//displayMatrix("2");
 	posPwmX = (kpX * posErrorX) + (kdX * (posErrorX - oldPosErrorX));
 	posPwmW = (kpW * posErrorW) + (kdW * (posErrorW - oldPosErrorW));
	
 	oldPosErrorX = posErrorX;
 	oldPosErrorW = posErrorW;
 	leftBaseSpeed = posPwmX - posPwmW;
 	rightBaseSpeed = posPwmX + posPwmW;
 	setLeftPwm(leftBaseSpeed);
 	setRightPwm(rightBaseSpeed);
}
Esempio n. 4
0
File: test.c Progetto: ktain/Slither
/**
 *	Hug Front Wall
 */
void hugFrontWall(int LSensorVal, int RSensorVal) {
	while (1) {
		int curt = micros(); //start to track time in order to make one adjust every 1000us
		readSensor();
		setLeftPwm(LSensorVal - LFSensor);
		setRightPwm(RSensorVal - RFSensor);
		elapseMicros(1000, curt); //elapse 1000 micro seconds
	}
}
Esempio n. 5
0
File: test.c Progetto: ktain/Slither
/*	
		Function: wheelOffsetTest()
		Parameters: motor speed, ontime
		Return: right - left encoder count
 */
int wheelOffsetTest(int speed, int ontime) {
	resetLeftEncCount();
	resetRightEncCount();

	setLeftPwm(speed);
	setRightPwm(speed);    
	delay_ms(ontime);
	turnMotorOff; 
	delay_ms(100);
	
	return getRightEncCount() - getLeftEncCount();
}
Esempio n. 6
0
void calculateMotorPwm(void) { // encoder PD controller
	int gyroFeedback = 0;
	int rotationalFeedback = 0;
	int sensorFeedback = 0;
	
  /* simple PD loop to generate base speed for both motors */	
	encoderFeedbackX = rightEncChange + leftEncChange;
	encoderFeedbackW = rightEncChange - leftEncChange;
	
	gyroFeedback = aSpeed/gyroFeedbackRatio; 	//gyroFeedbackRatio mentioned in curve turn lecture
	
	rotationalFeedback += encoderFeedbackW;
	
	
	if (useGyro || 1) {
		rotationalFeedback += gyroFeedback;
	}
	
	
	// option to include sensor feedback
		
	posErrorX += curSpeedX - encoderFeedbackX;
	posErrorW += curSpeedW - rotationalFeedback;
	
	if (useIRSensors) {
		getSensorError();
		sensorFeedback = sensorError/sensorScale;
		posErrorW += sensorFeedback;
	}
	
	posPwmX = kpX * posErrorX + kdX * (posErrorX - oldPosErrorX);
	posPwmW = kpW * posErrorW + kdW * (posErrorW - oldPosErrorW);
	
	oldPosErrorX = posErrorX;
	oldPosErrorW = posErrorW;
	
	leftBaseSpeed = posPwmX - posPwmW;
	rightBaseSpeed = posPwmX + posPwmW;

	setLeftPwm(leftBaseSpeed);
	setRightPwm(rightBaseSpeed);	
}
Esempio n. 7
0
int main(void) 
{
	int i;
	Systick_Configuration();
	LED_Configuration();
	button_Configuration();
	usart1_Configuration(9600);
	SPI_Configuration();
  TIM4_PWM_Init();
	Encoder_Configration();
	buzzer_Configuration();
	ADC_Config();
	
	//curSpeedX = 0;
	//curSpeedW = 0;
	//shortBeep(2000, 8000);
	
	while(1) {
		readSensor();
		readGyro();
		readVolMeter();
		printf("LF %d RF %d DL %d DR %d aSpeed %d angle %d voltage %d lenc %d renc %d\r\n", LFSensor, RFSensor, DLSensor, DRSensor, aSpeed, angle, voltage, getLeftEncCount(), getRightEncCount());
		displayMatrix("UCLA");
		
		setLeftPwm(100);
		setRightPwm(100);
		delay_ms(1000);
	}
	//forwardDistance(4000,0,0,true);
	//displayMatrix("Sped");
	//targetSpeedX = 100;
	//delay_ms(2000);
	
	//
	//targetSpeedX = 100;
	//delay_ms(2000);
	//printf("==============================================\n\r=======================================================\r\n");
	
	//delay_ms(1000); 
	//displayMatrix("Wat");
	//delay_ms(1000);
	//displayMatrix("STOP");
	//displayMatrix("GO");
	
	turnRightAngle(LEFT);
	
	targetSpeedW = 0;
	delay_ms(1000);
	turnRightAngle(RIGHT);
	targetSpeedW = 0;
	delay_ms(1000);
	
	displayMatrix("STOP");
	delay_ms(3000); 
	targetSpeedX = 0;
	//
	targetSpeedW = 10;
	delay_ms(1000);
	targetSpeedX = 0;
	targetSpeedW = 0;
	return 0;
}