예제 #1
0
파일: Drive.c 프로젝트: eiswasser/Sumobotic
static portTASK_FUNCTION(DriveTask, pvParameters) {
  (void)pvParameters; /* parameter not used */
  bool prevOn;
  //traceLabel usrEvent;

  prevOn = DRV_SpeedOn;
#if PL_HAS_RTOS_TRACE
  //usrEvent = xTraceOpenLabel("drive");
#endif
  for(;;) {
#if PL_HAS_RTOS_TRACE
    //RTOSTRC1_vTraceUserEvent(usrEvent);
#endif
    /*! \todo extend this for your own needs and with a position PID */
    TACHO_CalcSpeed();
    if (prevOn && !DRV_SpeedOn) { /* turned off */
      MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
      MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
      PID_Start(); /* reset values */
    } else if (DRV_SpeedOn) {
      PID_Speed(TACHO_GetSpeed(TRUE), DRV_SpeedLeft, TRUE); /* left */
      PID_Speed(TACHO_GetSpeed(FALSE), DRV_SpeedRight, FALSE); /* right */
    }
    prevOn = DRV_SpeedOn;
    FRTOS1_vTaskDelay(2/portTICK_RATE_MS);
  } /* for */
}
예제 #2
0
파일: Drive.c 프로젝트: sisem/intro
static void DriveTask(void *pvParameters) {
  portTickType xLastWakeTime;

  (void)pvParameters;
  xLastWakeTime = xTaskGetTickCount();
  for(;;) {
    while (GetCmd()==ERR_OK) { /* returns ERR_RXEMPTY if queue is empty */
      /* process incoming commands */
    }
    TACHO_CalcSpeed();
    if (DRV_Status.mode==DRV_MODE_SPEED) {
      PID_Speed(TACHO_GetSpeed(TRUE), DRV_Status.speed.left, TRUE);
      PID_Speed(TACHO_GetSpeed(FALSE), DRV_Status.speed.right, FALSE);
    } else if (DRV_Status.mode==DRV_MODE_STOP) {
      PID_Speed(TACHO_GetSpeed(TRUE), 0, TRUE);
      PID_Speed(TACHO_GetSpeed(FALSE), 0, FALSE);
    } else if (DRV_Status.mode==DRV_MODE_POS) {
      PID_Pos(Q4CLeft_GetPos(), DRV_Status.pos.left, TRUE);
      PID_Pos(Q4CRight_GetPos(), DRV_Status.pos.right, FALSE);
    } else if (DRV_Status.mode==DRV_MODE_NONE) {
      /* do nothing */
    }
    FRTOS1_vTaskDelayUntil(&xLastWakeTime, 5/portTICK_RATE_MS);
  } /* for */
}
예제 #3
0
/*!
 * \brief Prints the system low power status
 * \param io I/O channel to use for printing status
 */
static void TACHO_PrintStatus(const CLS1_StdIOType *io) {
  TACHO_CalcSpeed(); /*! \todo only temporary until this is done periodically */
  CLS1_SendStatusStr((unsigned char*)"Tacho", (unsigned char*)"\r\n", io->stdOut);
  CLS1_SendStatusStr((unsigned char*)"  L speed", (unsigned char*)"", io->stdOut);
  CLS1_SendNum32s(TACHO_GetSpeed(TRUE), io->stdOut);
  CLS1_SendStr((unsigned char*)" steps/sec\r\n", io->stdOut);
  CLS1_SendStatusStr((unsigned char*)"  R speed", (unsigned char*)"", io->stdOut);
  CLS1_SendNum32s(TACHO_GetSpeed(FALSE), io->stdOut);
  CLS1_SendStr((unsigned char*)" steps/sec\r\n", io->stdOut);
}
예제 #4
0
파일: Tacho.c 프로젝트: infotronik/sumo
void TACHO_Sample(void) {
#if 1
  static int cnt = 0;
  /* get called from the RTOS tick counter. Divide the frequency. */
  cnt += portTICK_RATE_MS;
  if (cnt < TACHO_SAMPLE_PERIOD_MS) {
    return;
  }
  cnt = 0; /* reset counter */
  /* left */
  TACHO_LeftPosHistory[TACHO_PosHistory_Index] = Q4CLeft_GetPos();
  TACHO_RightPosHistory[TACHO_PosHistory_Index] = Q4CRight_GetPos();
  TACHO_PosHistory_Index++;
  if (TACHO_PosHistory_Index >= NOF_HISTORY) {
    TACHO_PosHistory_Index = 0;
  }
  /* \todo Temporary only! You will need to do this later before doing PID calculation */
  TACHO_CalcSpeed();
#else
  /*! \todo Implement function */ 
#endif
}
예제 #5
0
파일: Drive.c 프로젝트: chregubr85/42
static portTASK_FUNCTION(DriveTask, pvParameters) {
  (void)pvParameters; /* parameter not used */
  bool prevOnS, prevOnP;

  uint32_t i;
  int32_t PosL,PosR;
  uint32_t dL,dR;
  uint32_t sL,sR;
  //traceLabel usrEvent;

  prevOnS = DRV_SpeedOn;
  prevOnP = DRV_PosOn;
  sL = 30000;
  sR = 30000;
#if PL_HAS_RTOS_TRACE
  //usrEvent = xTraceOpenLabel("drive");
#endif
  for(;;) {
#if PL_HAS_RTOS_TRACE
    //RTOSTRC1_vTraceUserEvent(usrEvent);
#endif
    /*! \todo extend this for your own needs and with a position PID */
    TACHO_CalcSpeed();
    if (prevOnS && !DRV_SpeedOn) { /* speed turned off */
      MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
      MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
      PID_Start(); /* reset values */
    } else if (DRV_SpeedOn && !DRV_PosOn) { /*speed control*/
      PID_Speed(TACHO_GetSpeed(TRUE), DRV_SpeedLeft, TRUE); /* left */
      PID_Speed(TACHO_GetSpeed(FALSE), DRV_SpeedRight, FALSE); /* right */
    } else if (DRV_PosOn && !DRV_SpeedOn) { /*pos control*/
      PosL = Q4CLeft_GetPos();
      PosR = Q4CRight_GetPos();
      dL = ((PosL-DRV_PosLeft)<=0) ? -(PosL-DRV_PosLeft) : (PosL-DRV_PosLeft);
      dR = ((PosR-DRV_PosRight)<=0) ? -(PosR-DRV_PosRight) : (PosR-DRV_PosRight);
      if(dL>100){
    	  if((DRV_PosLeft-PosL)>0){
    	  	  sL += 15;
    	  } else{
    	      sL -= 15;
    	  }
      } else{
    	  sL = DRV_PosLeft;
      }
      if(dR>100){
          	  if((DRV_PosRight-PosR)>0){
          	  	  sR += 15;
          	  } else{
          	      sR -= 15;
          	  }
            } else{
          	  sR = DRV_PosRight;
            }


      PID_Pos(PosL, sL, TRUE);		//left
      PID_Pos(PosR, sR, FALSE);	//right

      if((PosR < 1000) || (PosR > 63000)){  /*range safety*/
      	  DRV_PosRight = 30000;
      	  Q4CRight_SetPos(30000);
      }
      if((PosL < 1000) || (PosL > 63000)){
    	  DRV_PosLeft = 30000;
    	  Q4CLeft_SetPos(30000);
      }

    } else if(prevOnP && !DRV_PosOn){ /*pos turned off*/
    	MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
    	MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
    	for(i=0;i<100000;i++);
    	DRV_PosLeft = 30000;
    	DRV_PosRight = 30000;
    	Q4CLeft_SetPos(30000);
    	Q4CRight_SetPos(30000);
    }

    prevOnS = DRV_SpeedOn;
    prevOnP = DRV_PosOn;
    FRTOS1_vTaskDelay(2/portTICK_RATE_MS);
  } /* for */
}