コード例 #1
0
ファイル: Pid.c プロジェクト: danstadelmann/INTRO_CW
void PID_LineCfg(uint16_t currLine, uint16_t setLine, PID_Config *config) {
  int32_t pid, speed, speedL, speedR;
  MOT_Direction directionL=MOT_DIR_FORWARD, directionR=MOT_DIR_FORWARD;

  pid = PID(currLine, setLine, config);
  uint8_t errorPercent;
  errorPercent = errorWithinPercent(currLine-setLine);

  /* change speed depending on error */
  /* transform into different speed for motors. The PID is used as difference value to the motor PWM */
  if (errorPercent <= 20) { /* pretty on center: move forward both motors with base speed */
      speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
      pid = Limit(pid, -speed, speed);
      if (pid<0) { /* turn right */
        speedR = speed;
        speedL = speed-pid;
      } else { /* turn left */
        speedR = speed+pid;
        speedL = speed;
      }
   } else if (errorPercent <= 40) {
	   /* outside left/right halve position from center, slow down one motor and speed up the other */
	   speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*8/10; /* 80% */
	   pid = Limit(pid, -speed, speed);
	   if (pid<0) { /* turn right */
		   speedR = speed+pid; /* decrease speed */
		   speedL = speed-pid; /* increase speed */
	   } else { /* turn left */
		   speedR = speed+pid; /* increase speed */
		   speedL = speed-pid; /* decrease speed */
	   }
  } else if (errorPercent <= 70) {
	  speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*6/10; /* %60 */
      pid = Limit(pid, -speed, speed);
      if (pid<0) { /* turn right */
    	  speedR = 0 /*maxSpeed+pid*/; /* decrease speed */
    	  speedL = speed-pid; /* increase speed */
      } else { /* turn left */
    	  speedR = speed+pid; /* increase speed */
    	  speedL = 0 /*maxSpeed-pid*/; /* decrease speed */
      }
  } else  {
      /* line is far to the left or right: use backward motor motion */
      speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* %80 */
      if (pid<0) { /* turn right */
    	  speedR = -speed+pid; /* decrease speed */
    	  speedL = speed-pid; /* increase speed */
      } else { /* turn left */
    	  speedR = speed+pid; /* increase speed */
    	  speedL = -speed-pid; /* decrease speed */
      }
      speedL = Limit(speedL, -speed, speed);
      speedR = Limit(speedR, -speed, speed);
      directionL = AbsSpeed(&speedL);
      directionR = AbsSpeed(&speedR);
  }
  /* speed is now always positive, make sure it is within 16bit PWM boundary */
  if (speedL>0xFFFF) {
	  speedL = 0xFFFF;
  } else if (speedL<0) {
	  speedL = 0;
  }
  if (speedR>0xFFFF) {
	  speedR = 0xFFFF;
  } else if (speedR<0) {
      speedR = 0;
  }
  /* send new speed values to motor */
  MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
  MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
  MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
  MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
}
コード例 #2
0
ファイル: Pid.c プロジェクト: johnfab/IntroFabioStefanie
void PID_LineCfg(uint16_t currLine, uint16_t setLine, PID_Config *config) {
  int32_t pid, speed, speedL, speedR;
#if PID_DEBUG
  unsigned char buf[16];
  static uint8_t cnt = 0;
#endif
  uint8_t errorPercent;
  MOT_Direction directionL=MOT_DIR_FORWARD, directionR=MOT_DIR_FORWARD;

  pid = PID(currLine, setLine, config);
  errorPercent = errorWithinPercent(currLine-setLine);

  /* transform into different speed for motors. The PID is used as difference value to the motor PWM */
  if (errorPercent <= 20) { /* pretty on center: move forward both motors with base speed */
    speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
    pid = Limit(pid, -speed, speed);
    if (pid<0) { /* turn right */
      speedR = speed;
      speedL = speed-pid;
    } else { /* turn left */
      speedR = speed+pid;
      speedL = speed;
    }
  } else if (errorPercent <= 40) {
    /* outside left/right halve position from center, slow down one motor and speed up the other */
    speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*8/10; /* 80% */
    pid = Limit(pid, -speed, speed);
    if (pid<0) { /* turn right */
      speedR = speed+pid; /* decrease speed */
      speedL = speed-pid; /* increase speed */
    } else { /* turn left */
      speedR = speed+pid; /* increase speed */
      speedL = speed-pid; /* decrease speed */
    }
  } else if (errorPercent <= 70) {
    speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*6/10; /* %60 */
    pid = Limit(pid, -speed, speed);
    if (pid<0) { /* turn right */
      speedR = 0 /*maxSpeed+pid*/; /* decrease speed */
      speedL = speed-pid; /* increase speed */
    } else { /* turn left */
      speedR = speed+pid; /* increase speed */
      speedL = 0 /*maxSpeed-pid*/; /* decrease speed */
    }
  } else  {
    /* line is far to the left or right: use backward motor motion */
    speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* %80 */
    if (pid<0) { /* turn right */
      speedR = -speed+pid; /* decrease speed */
      speedL = speed-pid; /* increase speed */
    } else { /* turn left */
      speedR = speed+pid; /* increase speed */
      speedL = -speed-pid; /* decrease speed */
    }
    speedL = Limit(speedL, -speed, speed);
    speedR = Limit(speedR, -speed, speed);
    directionL = AbsSpeed(&speedL);
    directionR = AbsSpeed(&speedR);
  }
  /* speed is now always positive, make sure it is within 16bit PWM boundary */
  if (speedL>0xFFFF) {
    speedL = 0xFFFF;
  } else if (speedL<0) {
    speedL = 0;
  }
  if (speedR>0xFFFF) {
    speedR = 0xFFFF;
  } else if (speedR<0) {
    speedR = 0;
  }
  /* send new speed values to motor */
  MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
  MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
  MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
  MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
#if PID_DEBUG /* debug diagnostic */
  {
    cnt++;
    if (cnt>10) { /* limit number of messages to the console */
      CLS1_StdIO_OutErr_FctType ioOut = CLS1_GetStdio()->stdOut;
      cnt = 0;

      CLS1_SendStr((unsigned char*)"line:", ioOut);
      buf[0] = '\0';
      UTIL1_strcatNum16u(buf, sizeof(buf), currLine);
      CLS1_SendStr(buf, ioOut);

      CLS1_SendStr((unsigned char*)" sum:", ioOut);
      buf[0] = '\0';
      UTIL1_strcatNum32Hex(buf, sizeof(buf), integral);
      CLS1_SendStr(buf, ioOut);

      CLS1_SendStr((unsigned char*)" left:", ioOut);
      CLS1_SendStr(directionL==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
      buf[0] = '\0';
      UTIL1_strcatNum16Hex(buf, sizeof(buf), speedL);
      CLS1_SendStr(buf, ioOut);

      CLS1_SendStr((unsigned char*)" right:", ioOut);
      CLS1_SendStr(directionR==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
      buf[0] = '\0';
      UTIL1_strcatNum16Hex(buf, sizeof(buf), speedR);
      CLS1_SendStr(buf, ioOut);

      CLS1_SendStr((unsigned char*)"\r\n", ioOut);
    }
  }
#endif
}