/* motor.c */

/* motor controller code */

int motor_speed[6];

/* initialize motors */
int init_motors()
{
  int i;
  for(i=0;i<6;i++)
  {
    motor_speed[i]=0;
    motor(i, 0);
  }
  return 0;
}

int get_motor_speed(int motor_id)
{
#ifdef DEBUG
  if (motor_id<0 || motor_id>5)
  {
    printf("get_motor_speed: invalid id %d\n", motor_id);
    return 1;
  }
#endif
  return motor_speed[motor_id];
}

int set_motor_speed(int motor_id, int speed)
{
#ifdef DEBUG
  if (motor_id<0 || motor_id>5)
  {
    printf("set_motor: invalid motor id %d\n", motor_id);
    return 1;
  }
  if (speed<-100 || speed>100)
  {
    printf("set_motor: invalid speed %d\n", speed);
    return 1;
  }
#endif
  motor_speed[motor_id] = speed;
  if (motor_id==MOTOR_FRONT_LEFT || motor_id==MOTOR_REAR_LEFT)
    motor(motor_id, speed);
  else
    motor(motor_id, -speed);
}