void CalculatedSpeed() {
if(potVoltage > 512){
setLeftWheelSpeed(1023);
setRightWheelSpeed(2048 - potVoltage * 2);
}
else if (potVoltage == 512){
setLeftWheelSpeed(1023);
setRightWheelSpeed(1023);
}
else{
setLeftWheelSpeed(potVoltage * 2);
setRightWheelSpeed(1023);
}
}
//едем назад
void ARobotic::moveBackward(uint8_t speed, uint16_t distance){
digitalWrite(DIRL,LOW);
digitalWrite(DIRR,LOW);
if (speed){
setRightWheelSpeed(speed);
setLeftWheelSpeed(speed);
}
//обработка distance
if (distance){
waitingDistance4LM(distance);
stopMoving();
}
}
int main(void) {
SYSTEMConfigPerformance(10000000);
enableInterrupts(); //This function is necessary to use interrupts.
initTimer2();
initTimer3();
switch1();
initPWM();
initADC();
initLCD();
state = IDLE_1;
volatile float printbuffer = 0; //maybe change to float
// double voltPOT = 0;
char str[16];
while(1){
// clearLCD();
// snprintf(str, sizeof(str), "%0.2f", (float)potVoltage/1023.0*5.0);
// printStringLCD(str);
// moveCursorLCD(0,0);
switch(state){
voltage = potVoltage*1.0;
case IDLE_1:
setLeftForward(1);
setRightForward(1);
setLeftWheelSpeed(0);
setRightWheelSpeed(0);
break;
case D_IDLE_1:
delayUs(100);
state = FORWARD;
break;
case FORWARD:
CalculatedSpeed();
AD1CON1bits.SAMP = 1;
break;
case D_FORWARD:
delayUs(100);
state = IDLE_2;
break;
case IDLE_2:
setLeftWheelSpeed(0);
setRightWheelSpeed(0);
break;
case D_IDLE_2:
delayUs(100);
state = BACKWARD;
break;
case BACKWARD:
setLeftForward(0);
setRightForward(0);
CalculatedSpeed();
AD1CON1bits.SAMP = 1;
break;
case D_BACKWARD:
delayUs(100);
state = IDLE_1;
break;
}
}
return 0;
}
