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