/**
* Move half cell at stopspeed
*/
void moveForwardHalf(void) {
useIRSensors = 1;
useSpeedProfile = 1;
if (useGyroCorrection)
useGyro = 1;
targetSpeedX = stopSpeed;
int startEncCount = (getLeftEncCount() + getRightEncCount()) / 2;
while( (getLeftEncCount() + getRightEncCount()) / 2 < startEncCount + cellDistance/2 ) {
targetSpeedX = stopSpeed;
}
targetSpeedX = stopSpeed;
useGyro = 0;
}
/**
* Straight movement
*/
void moveForward(float cells) {
if (cells < 0) {
cells = 0;
}
useIRSensors = 1;
useSpeedProfile = 1;
int startEncCount = (getLeftEncCount() + getRightEncCount()) / 2;
int remainingDist = cells*cellDistance;
while( encCount - startEncCount < cells*cellDistance ) {
remainingDist = cells*cellDistance - (encCount - startEncCount);
if (remainingDist < cellDistance/2) {
useIRSensors = 0;
}
if (getDecNeeded(counts_to_mm(remainingDist), curSpeedX, stopSpeed) < decX) {
targetSpeedX = moveSpeed;
}
else {
targetSpeedX = stopSpeed;
}
}
targetSpeedX = stopSpeed;
}
/*
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();
}
void getEncoderStatus()
{
//displayMatrix("Enc");
leftEncoder = getLeftEncCount();
rightEncoder = getRightEncCount();
leftEncoderChange = leftEncoder - leftEncoderOld;
rightEncoderChange = rightEncoder - rightEncoderOld;
encoderChange = (leftEncoderChange + rightEncoderChange)/2;
leftEncoderOld = leftEncoder;
rightEncoderOld = rightEncoder;
leftEncoderCount += leftEncoderChange;
rightEncoderCount += rightEncoderChange;
encoderCount = (leftEncoderCount + rightEncoderCount)/2;
distanceLeft -= encoderChange;
}
// Updates encoder counts and distance left
void getEncoderStatus(void)
{
leftEncCount = getLeftEncCount();
rightEncCount = getRightEncCount();
leftEncChange = leftEncCount - leftEncOld;
rightEncChange = rightEncCount - rightEncOld;
encChange = (leftEncChange + rightEncChange)/2;
leftEncOld = leftEncCount;
rightEncOld = rightEncCount;
leftEncCount += leftEncChange;
rightEncCount += rightEncChange;
encCount = (leftEncCount + rightEncCount)/2;
distanceLeft -= encChange; // update distanceLeft
}
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;
}