int calibrate_tt(Turntable_t* turntable)
{
// This function must be run immediately after reset to calibrate the encoder readings
// Reset max encoder value
turntable->maxEncoder = 0;
int prev_encoder = bcm2835_gpio_lev(ENCODER);
motorStart(1);
while(!bcm2835_gpio_lev(SWITCH))
{
// Waits for falling edge
if(prev_encoder != bcm2835_gpio_lev(ENCODER))
{
prev_encoder = !prev_encoder;
turntable->maxEncoder++;
}
}
while(bcm2835_gpio_lev(SWITCH))
{
// waits for rising edge
if(prev_encoder != bcm2835_gpio_lev(ENCODER))
{
prev_encoder = !prev_encoder;
turntable->maxEncoder++;
}
}
motorStop();
// reset current encoder to start
turntable->currentEncoder = 0;
printf("Max encoder = %d\r\n", turntable->maxEncoder);
return 0;
}
int quarterTurn_tt(Turntable_t* turntable, int direction, int quarters)
{
int target = turntable->currentEncoder + (turntable->maxEncoder/4)*quarters;
int prev_encoder = bcm2835_gpio_lev(ENCODER);
printf("target = %d\r\ncurrentEncoder = %d\r\n",target,turntable->currentEncoder);
motorStart(direction);
while(target > turntable->currentEncoder)
{
if(prev_encoder != bcm2835_gpio_lev(ENCODER))
{
prev_encoder = !prev_encoder;
turntable->currentEncoder++;
}
if(!bcm2835_gpio_lev(SWITCH))
{
if(target > turntable->maxEncoder)
{
target = target - turntable->maxEncoder;
turntable->currentEncoder = 0;
break;
}
}
}
motorStop();
return 0;
}
// Movimientos compuestos
void Motor::move(char _direction)
{
switch(_direction)
{
case 'w':
{
motorStart(1, _motorDirForward);
setSpeed(1, 220);
motorStart(2, _motorDirForward);
setSpeed(2, 220);
}
break;
case 's':
{
motorStart(1, _motorDirBackward);
setSpeed(1, 220);
motorStart(2, _motorDirBackward);
setSpeed(2, 220);
}
break;
case 'a':
{
motorStart(1, _motorDirBackward);
setSpeed(1, 180);
motorStart(2, _motorDirForward);
setSpeed(2, 180);
}
break;
case 'd':
{
motorStart(1, _motorDirForward);
setSpeed(1, 180);
motorStart(2, _motorDirBackward);
setSpeed(2, 180);
}
break;
case 'q':
{
motorStop(1);
motorStop(2);
}
break;
}
}
int reset_tt(Turntable_t* turntable)
{
printf("Start reset\r\n");
// This function returns the turntable to its starting state
motorStart(1);
// Wait for reset
while(!bcm2835_gpio_lev(SWITCH))
{
// Waits for falling edge
}
while(bcm2835_gpio_lev(SWITCH))
{
// waits for rising edge
}
motorStop();
// Reset the turntable valus
turntable->maxEncoder = 0;
turntable->currentEncoder = 0;
printf("Finish reset\r\n");
return 0;
}
int fineTurn_tt(Turntable_t* turntable, int direction, int ticks)
{
int target = turntable->currentEncoder + ticks;
int prev_encoder = bcm2835_gpio_lev(ENCODER);
motorStart(direction);
while(target > turntable->currentEncoder)
{
if(prev_encoder != bcm2835_gpio_lev(ENCODER))
{
prev_encoder = !prev_encoder;
turntable->maxEncoder++;
}
if(turntable->currentEncoder > turntable->maxEncoder*3/4 && bcm2835_gpio_lev(SWITCH) == 1)
{
target = target - turntable->maxEncoder;
turntable->currentEncoder = 0;
}
}
motorStop();
return 0;
}
