Пример #1
0
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 */
}
Пример #2
0
static void DRV_PrintStatus(const CLS1_StdIOType *io) {
  uint8_t buf[32];
  
  CLS1_SendStatusStr((unsigned char*)"drive", (unsigned char*)"\r\n", io->stdOut);
  CLS1_SendStatusStr((unsigned char*)"  speed", DRV_SpeedOn?(unsigned char*)"on\r\n":(unsigned char*)"off\r\n", io->stdOut);
  buf[0]='\0';
  UTIL1_strcatNum32s(buf, sizeof(buf), DRV_SpeedLeft);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
  CLS1_SendStatusStr((unsigned char*)"  speed L", buf, io->stdOut);
  buf[0]='\0';
  UTIL1_strcatNum32s(buf, sizeof(buf), DRV_SpeedRight);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n\r\n");
  CLS1_SendStatusStr((unsigned char*)"  speed R", buf, io->stdOut);
  buf[0]='\0';
  CLS1_SendStatusStr((unsigned char*)"  pos", DRV_PosOn?(unsigned char*)"on\r\n":(unsigned char*)"off\r\n", io->stdOut);
  buf[0]='\0';
  UTIL1_strcatNum32s(buf, sizeof(buf), DRV_PosLeft);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
  CLS1_SendStatusStr((unsigned char*)"  pos L", buf, io->stdOut);
  buf[0]='\0';
  UTIL1_strcatNum32s(buf, sizeof(buf), Q4CLeft_GetPos());
  CLS1_SendStatusStr((unsigned char*)"  pos L Q4", buf, io->stdOut);
  CLS1_SendStr((unsigned char*)"\r\n",io->stdOut);
  buf[0]='\0';
  UTIL1_strcatNum32s(buf, sizeof(buf), DRV_PosRight);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
  CLS1_SendStatusStr((unsigned char*)"  pos R", buf, io->stdOut);
  buf[0]='\0';
  UTIL1_strcatNum32s(buf, sizeof(buf), Q4CRight_GetPos());
  CLS1_SendStatusStr((unsigned char*)"  pos R Q4", buf, io->stdOut);
  CLS1_SendStr((unsigned char*)"\r\n",io->stdOut);
}
Пример #3
0
static void DRV_PrintStatus(const CLS1_StdIOType *io) {
  uint8_t buf[48];

  CLS1_SendStatusStr((unsigned char*)"drive", (unsigned char*)"\r\n", io->stdOut);

  CLS1_SendStatusStr((unsigned char*)"  mode", DRV_GetModeStr(DRV_Status.mode), io->stdOut);
  CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);

  UTIL1_Num32sToStr(buf, sizeof(buf), DRV_Status.speed.left);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" steps/sec (curr: ");
  UTIL1_strcatNum32s(buf, sizeof(buf), TACHO_GetSpeed(TRUE));
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)")\r\n");
  CLS1_SendStatusStr((unsigned char*)"  speed left", buf, io->stdOut);

  UTIL1_Num32sToStr(buf, sizeof(buf), DRV_Status.speed.right);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" steps/sec (curr: ");
  UTIL1_strcatNum32s(buf, sizeof(buf), TACHO_GetSpeed(FALSE));
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)")\r\n");
  CLS1_SendStatusStr((unsigned char*)"  speed right", buf, io->stdOut);

  UTIL1_Num32sToStr(buf, sizeof(buf), DRV_Status.pos.left);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" (curr: ");
  UTIL1_strcatNum32s(buf, sizeof(buf), (int32_t)Q4CLeft_GetPos());
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)")\r\n");
  CLS1_SendStatusStr((unsigned char*)"  pos left", buf, io->stdOut);

  UTIL1_Num32sToStr(buf, sizeof(buf), DRV_Status.pos.right);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" (curr: ");
  UTIL1_strcatNum32s(buf, sizeof(buf), (int32_t)Q4CRight_GetPos());
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)")\r\n");
  CLS1_SendStatusStr((unsigned char*)"  pos right", buf, io->stdOut);
}
Пример #4
0
bool DRV_HasTurned(void) {
  int16_t pos;

  if (FRTOS1_uxQueueMessagesWaiting(DRV_Queue)>0) {
    return FALSE; /* still messages in command queue, so there is something pending */
  }
  if (DRV_Status.mode==DRV_MODE_POS) {
    #define DRV_TURN_SPEED_LOW 50
    int32_t speedL, speedR;

    speedL = TACHO_GetSpeed(TRUE);
    speedR = TACHO_GetSpeed(FALSE);
    if (speedL>-DRV_TURN_SPEED_LOW && speedL<DRV_TURN_SPEED_LOW && speedR>-DRV_TURN_SPEED_LOW && speedR<DRV_TURN_SPEED_LOW) { /* speed close to zero */
      pos = Q4CLeft_GetPos();
      if (match(pos, DRV_Status.pos.left)) {
        pos = Q4CRight_GetPos();
        if (match(pos, DRV_Status.pos.right)) {
          return TRUE;
        }
      }
    }
    return FALSE;
  } /* if */
  return TRUE;
}
Пример #5
0
void TURN_TurnSteps(int16_t stepsL, int16_t stepsR) {
  int16_t currLPos, currRPos, targetLPos, targetRPos;
  uint8_t matchCntr = 100;
  
  currLPos = Q4CLeft_GetPos();
  targetLPos = currLPos+stepsL;
  currRPos = Q4CRight_GetPos();
  targetRPos = currRPos+stepsR;
  for(;;) { /* breaks */
    if (match(currLPos,targetLPos) && match(currRPos,targetRPos)) {
      matchCntr--;
      if (matchCntr==0) {
        break; /* get out of loop */
      }
    }
    PID_Pos(Q4CLeft_GetPos(), targetLPos, TRUE);
    PID_Pos(Q4CRight_GetPos(), targetRPos, FALSE);
    WAIT1_WaitOSms(2);
    currLPos = Q4CLeft_GetPos();
    currRPos = Q4CRight_GetPos();
  } /* for */
}
Пример #6
0
void TACHO_Sample(void) {
  /*! \todo Implement/change function as needed, make sure implementation below matches your needs */
  static int cnt = 0;
  /* get called from the RTOS tick counter. Divide the frequency. */
  cnt += TMR_TICK_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;
  }
}
Пример #7
0
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
}
Пример #8
0
static void TURN_PrintStatus(const CLS1_StdIOType *io) {
  unsigned char buf[32];

  CLS1_SendStatusStr((unsigned char*)"turn", (unsigned char*)"\r\n", io->stdOut);

#if PL_HAS_QUADRATURE
  UTIL1_Num16uToStr(buf, sizeof(buf), TURN_Steps90);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" steps\r\n");
  CLS1_SendStatusStr((unsigned char*)"  steps90", buf, io->stdOut);

  UTIL1_Num16uToStr(buf, sizeof(buf), TURN_StepsFwBw);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" steps\r\n");
  CLS1_SendStatusStr((unsigned char*)"  stepsfw", buf, io->stdOut);

  UTIL1_Num16sToStr(buf, sizeof(buf), Q4CLeft_GetPos());
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)", ");
  UTIL1_strcatNum16u(buf, sizeof(buf), Q4CLeft_NofErrors());
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" errors\r\n");
  CLS1_SendStatusStr((unsigned char*)"  left pos", buf, io->stdOut);
  
  UTIL1_Num16sToStr(buf, sizeof(buf), Q4CRight_GetPos());
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)", ");
  UTIL1_strcatNum16u(buf, sizeof(buf), Q4CRight_NofErrors());
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" errors\r\n");
  CLS1_SendStatusStr((unsigned char*)"  right pos", buf, io->stdOut);
#else
  UTIL1_Num16uToStr(buf, sizeof(buf), TURN_DutyPercent);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"%\r\n");
  CLS1_SendStatusStr((unsigned char*)"  duty", buf, io->stdOut);

  UTIL1_Num16uToStr(buf, sizeof(buf), TURN_Time90ms);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" ms\r\n");
  CLS1_SendStatusStr((unsigned char*)"  time90", buf, io->stdOut);

  UTIL1_Num16uToStr(buf, sizeof(buf), TURN_StepFwBwMs);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" ms\r\n");
  CLS1_SendStatusStr((unsigned char*)"  timefw", buf, io->stdOut);
#endif
}
Пример #9
0
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 < SPEED_CALC_PERIOD_MS) { // Sampling alle 5 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;
  }
  /* temporary only */
 // TACHO_CalcSpeed();
#else
  /*! \todo Implement function */ 
#endif
}
Пример #10
0
bool DRV_IsStopped(void) {
  Q4CLeft_QuadCntrType leftPos;
  Q4CRight_QuadCntrType rightPos;

  if (FRTOS1_uxQueueMessagesWaiting(DRV_Queue)>0) {
    return FALSE; /* still messages in command queue, so there is something pending */
  }
  /* do *not* use/calculate speed: too slow! Use position encoder instead */
  leftPos = Q4CLeft_GetPos();
  rightPos = Q4CRight_GetPos();
  if (DRV_Status.mode==DRV_MODE_POS) {
    if (DRV_Status.pos.left!=(int32_t)leftPos) {
      return FALSE;
    }
    if (DRV_Status.pos.right!=(int32_t)rightPos) {
      return FALSE;
    }
    return TRUE;
  } else {
    /* ???? what to do otherwise ???? */
    return FALSE;
  }
}
Пример #11
0
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 */
}
Пример #12
0
static portTASK_FUNCTION(TraceTask, pvParameters) {
    unsigned char buf[32];

    (void)pvParameters;
    //ACCEL_LowLevelInit(); /* cannot do this in ACCEL_Init(), as there interrupts are disabled */
    for(;;) {
        if (traceChannel==TRACE_TO_NONE) {
            /* do nothing */
        } else if (traceChannel==TRACE_TO_SHELL) {
            buf[0] = '\0';
            UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" => ");
#if PL_HAS_MOTOR
            if (traceMotor) {
                buf[0] = '\0';
                if (MOT_GetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT))==MOT_DIR_FORWARD) {
                    UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: fw 0x");
                } else {
                    UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: bw 0x");
                }
                UTIL1_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT)));
                UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
                SHELL_SendString(&buf[0]);
#endif
            }
#endif
#if PL_HAS_MOTOR_TACHO
            if (traceTacho) {
                buf[0] = '\0';
                UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" V:");
                UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(FALSE), ' ', 6);
                UTIL1_chcat(buf, sizeof(buf), ' ');
                UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(TRUE), ' ', 6);
                UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
                SHELL_SendString(&buf[0]);
#endif
            }
#endif
#if PL_HAS_QUADRATURE
            if (traceQuad) {
                buf[0] = '\0';
                UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" P:");
                UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_GetPos(), ' ', 6);
                UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_GetPos(), ' ', 6);
                UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; E:");
                UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_NofErrors(), ' ', 6);
                UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_NofErrors(), ' ', 6);
                UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
                SHELL_SendString(&buf[0]);
#endif
            }
#endif
#if PL_HAS_ACCEL
            if (traceAccel) {
                int16_t x, y, z;

                buf[0] = '\0';
                ACCEL_GetValues(&x, &y, &z);
                UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" X:");
                UTIL1_strcatNum16sFormatted(buf, sizeof(buf), x, ' ', 6);
                UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Y:");
                UTIL1_strcatNum16sFormatted(buf, sizeof(buf), y, ' ', 6);
                UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Z:");
                UTIL1_strcatNum16sFormatted(buf, sizeof(buf), z, ' ', 6);
                UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
                SHELL_SendString(&buf[0]);
#endif
            }
#endif
            UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"\r\n");
#if PL_HAS_SHELL
            SHELL_SendString(&buf[0]);
#endif
            FRTOS1_vTaskDelay(20/portTICK_RATE_MS); /* slow down writing to console */
        }
        FRTOS1_vTaskDelay(20/portTICK_RATE_MS);
    } /* for */
}