void ResetEncoder(Encoder * me) {
DisableEncoder(me);
me->prevEncoderCount = 0;
me->currEncoderCount = 0;
me->deltaEncoderCount = 0;
me->accumEncoderCounts = 0;
TIM_SetCounter(me->encoderTIM, 0);
GetEncoderCounts(me);
EnableEncoder(me);
}
void arc_old(int radius, int degree) {
long l = 0, r = 0, avg = 0;//ol = 0, ort = 0;
PresetEncoderCounts(0,0);
//long long break_val = epr * radius * degree * 314;
UARTprintf("Hello, arc!\n");
//UARTprintf("%10d\n",break_val);
UARTprintf("%10d\n",BOT_WIDTH);
if (ABS(radius) < BOT_WIDTH/2)
return;
while(epr * radius * degree > 360 * WHEEL_RAD * avg)
{
GetEncoderCounts(&l,&r);
//pid(radius+BOT_WIDTH>>1,radius-BOT_WIDTH>>1);
avg = (l+r)>>1;
UARTprintf("Average: %10d %10d %10d %10d",(radius + BOT_WIDTH>>1),(radius - BOT_WIDTH>>1),epr * radius * degree,360*WHEEL_RAD*avg);
//ol = l; ort = r;
}
}
void rotate_enc(int degree) {
int ldiff = 0, rdiff = 0;
long l = 0, r = 0, count = 0;//oldl = 0, oldr = 0,
long break_val = (BOT_WIDTH * ABS(degree)) * (epr/(WHEEL_RAD*180));
PresetEncoderCounts(0,0);
if (degree > 0) SetMotorPowers(80,-80);
else SetMotorPowers(-80,80);
while (count * 4 < break_val)
{
GetEncoderCounts(&l, &r);
ldiff = (l>r)?0:r-l;
rdiff = (r>l)?0:l-r;
SetMotorPowers(127-ldiff,-128+rdiff);
count = (ABS(l)+ABS(r))/2;
UARTprintf("Count: %10d %10d\r",count,break_val);
//oldl = l; oldr = r;
WaitUS(1000);
}
}
void encoderDemo(void) {
UARTprintf("Press:\nany key-read encoder values\n");
UARTprintf("any key after read begins-quit\n");
{
signed long encoder0, encoder1, counter = 0;
PresetEncoderCounts(0, 0);
while(!keyWasPressed()) {
encoder0 = GetEncoderCount(ENCODER_0);
encoder1 = GetEncoderCount(ENCODER_1);
GetEncoderCounts(&encoder0, &encoder1);
UARTprintf("enc0:%d enc1:%d \r",encoder0,encoder1);
counter++;
}
}
UARTprintf("\n");
}
void move_enc(int distance) {
long enc0 = 0, enc1 = 0;
long oldr = 0, oldl = 0, count = 0;
long break_val = distance * epr /WHEEL_CIRC;
PresetEncoderCounts(0,0);
SetMotorPowers(127,127);
while(count < break_val)
{
GetEncoderCounts(&enc0, &enc1);
pid((enc0-oldr),(enc1-oldl));
oldr = enc0;
oldl = enc1;
count = (enc0 + enc1)/2;
//UARTprintf("Count: %10d %10d\r",WHEEL_CIRC*count,distance*epr);
//WaitUS(100);
}
SetMotorPowers(0,0);
return;
}
void arc(int radius, int degree) {
long l = 0, r = 0, avg = 0;//, ol = 0, ort = 0;
long long break_val = 0;
PresetEncoderCounts(0,0);
break_val = (epr * ABS(radius));
UARTprintf("Rad: %3d %d\n",ABS(radius),break_val);
//if (ABS(radius) < BOT_WIDTH/2)
// return;
while(break_val > WHEEL_RAD * avg * 180 * degree)
{
GetEncoderCounts(&l,&r);
//pid(radius+BOT_WIDTH,radius-BOT_WIDTH);
//pid(radius,BOT_WIDTH);
SetMotorPowers(MEDIAN(0,127,127 * (radius - BOT_WIDTH/2) / (radius + BOT_WIDTH/2)),
MEDIAN(0,127,127*(radius + BOT_WIDTH/2) / (radius - BOT_WIDTH/2)));
avg = (l+r)/2;
UARTprintf("Compare: %10d %10d Powers2: %5d %5d\r",break_val, WHEEL_RAD * avg * degree,
127*(radius + BOT_WIDTH/2) / (radius - BOT_WIDTH/2),127*(radius - BOT_WIDTH/2) / (radius + BOT_WIDTH/2));
//UARTprintf("Average: %10d %10d\r",(radius + BOT_WIDTH>>1),(radius - BOT_WIDTH>>1));
//ol = l; ort = r;
}
SetMotorPowers(0,0);
}