/** * 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; }