void InitEncoders()
{
reset_encoder(0);
reset_encoder(1);
fStalled = 0;
fBlocked = 0;
cL = -10;
cR = -10;
}
void back_turn_right (float rangle, int tstop, int lspeed, int rspeed)
{
int clicks_to_move;
int stval; // start time value
int dtval; // delta time value
reset_encoder();
// Now lets do a two wheel turn
// remember going backwards
clicks_to_move = compute_click_for_two_wheelturn(Wheel_Base, Wheel_Size, rangle);
time1[T1] = 0; // Start the timer at 0 msec
stval = time1[T1]; // get number of msec
dtval = 0; // compute delta time
// again remember depending on which way to want to
// turn, enable the correct motor direction
motor[Right1] = -rspeed;
motor[Right2] = -rspeed;
motor[Left1] = lspeed;
motor[Left2] = lspeed;
while ((nMotorEncoder[Left1] < clicks_to_move) && (dtval < tstop)) {
dtval = time1[T1] - stval;
wait1Msec(1);
}
stop_all_motors();
}
void move_backwards(float dist, int tstop, int lspeed, int rspeed)
{
int clicks_to_move;
int stval; // start time value
int dtval; // delta time value
reset_encoder();
// Move to the goal
// remember going backwards
clicks_to_move = -compute_clicks_per_distance(dist, Wheel_Size);
time1[T1] = 0; // Start the timer at 0 msec
stval = time1[T1]; // get number of msec
dtval = 0; // compute delta time
motor[Left1] = -lspeed;
motor[Left2] = -lspeed;
motor[Right1] = -rspeed;
motor[Right2] = -rspeed;
while ((nMotorEncoder[Left1] > clicks_to_move) && (dtval < tstop)) {
dtval = time1[T1] - stval;
wait1Msec(1);
}
stop_all_motors();
}
void move_forward(float dist, int tstop, int lspeed, int rspeed)
{
int clicks_to_move;
int stval; // start time value
int dtval; // delta time value
//int encdiff;
reset_encoder();
// Move away from the wall
clicks_to_move = compute_clicks_per_distance(dist, Wheel_Size);
// Start the timer to make sure we dont overshoot
// stop the motors if we never get to our
// distance goal so we don't burn out the motors
//
time1[T1] = 0; // Start the timer at 0 msec
stval = time1[T1]; // get number of msec
dtval = 0; // compute delta time
motor[Left1] = lspeed;
motor[Left2] = lspeed;
motor[Right1] = rspeed;
motor[Right2] = rspeed;
while ((nMotorEncoder[Right1] < clicks_to_move) && (dtval < tstop)) {
dtval = time1[T1] - stval;
wait1Msec(1);
}
stop_all_motors();
}
main(int argc, char* argv[])
{
if (argc != 4)
{
fprintf(stderr, "\nUsage: itu_encoder -[a|u] <PCM-File> <ADPCM-File>");
exit(-1);
}
I_file = argv[2];
O_file = argv[3];
put_sample(1);
reset_encoder();
LAW = argv[1][1] == 'u' ? u_LAW : A_LAW;
ExitFlag = FALSE;
while (!ExitFlag)
{
get_sample();
if (ExitFlag)
break;
O = encoder(I);
put_sample(2);
}
put_sample(3);
return (0);
}
void calibrate_encoder(){
controll_motor(-80); //Find left boundary
delay(3000);
reset_encoder();
controll_motor(80); //Find right boundary
delay(3000);
max_encoder_val = read_encoder();
controll_motor(0);
}
int main(void)
{
int i, sic, eic;
U16BIT I;
reset_encoder();
reset_decoder();
sic = __time / 2;
for (i = 0; i < sizeof(Input) / sizeof(U16BIT); i++)
decoder(encoder(Input[i]));
eic = __time / 2;
printf("\nInstruction cycles for transcoding a sample: %d\n",
(eic - sic) / (sizeof(Input) / sizeof(U16BIT)));
return (0);
}
void motor_init(){
DAC_init();
//Set port K as input
DDRK = 0x00;
pinMode(OE, OUTPUT);
pinMode(RST, OUTPUT);
pinMode(SEL, OUTPUT);
pinMode(EN, OUTPUT);
pinMode(DIR, OUTPUT);
//Enable motor
digitalWrite(EN, HIGH);
//Set !OE high to disable output of encoder
digitalWrite(OE, HIGH);
//Set !RST
reset_encoder();
//Calibrate
calibrate_encoder();
controll_motor(0);
}
// Move forward making sure encoder values are the same
void nmf(float dist, int tstop, int lspeed, int rspeed)
{
int clicks_to_move;
int stval; // start time value
int dtval; // delta time value
int encdiff;
reset_encoder();
// Move away from the wall
clicks_to_move = compute_clicks_per_distance(dist, Wheel_Size);
// Start the timer to make sure we dont overshoot
// stop the motors if we never get to our
// distance goal so we don't burn out the motors
//
time1[T1] = 0; // Start the timer at 0 msec
stval = time1[T1]; // get number of msec
dtval = 0; // compute delta time
motor[Left1] = lspeed;
motor[Left2] = lspeed;
motor[Right1] = rspeed;
motor[Right2] = rspeed;
while ((nMotorEncoder[Left1] < clicks_to_move) && (dtval < tstop))
{
encdiff = nMotorEncoder[Right1] - nMotorEncoder[Left1];
// if rightencoder > leftencoder
// slow down RightMotor
if (encdiff > 20)
{
motor[Right1] = rspeed - 20;
motor[Right2] = rspeed - 20;
}
else if (encdiff > 10)
{
motor[Right1] = rspeed - 10;
motor[Right2] = rspeed - 10;
}
// if rightencoder > leftencoder
// slow down leftmotor
else if (encdiff < -20)
{
motor[Left1] = lspeed - 20;
motor[Left2] = lspeed - 20;
}
else if (encdiff < -10)
{
motor[Left1] = lspeed - 10;
motor[Left2] = lspeed - 10;
}
else
{
motor[Left1] = lspeed;
motor[Left2] = lspeed;
motor[Right1] = rspeed;
motor[Right2] = rspeed;
}
dtval = time1[T1] - stval;
wait1Msec(1);
}
stop_all_motors();
}
