// max duty von 44% um max spannung von 50V am schaltnetzteil nicht zu übersteigen
void setPWM(int duty) {
if (duty > 4400) {
PWM_SetDutyCycle(&LED, 4400);
return;
} else if (duty < 0) {
PWM_SetDutyCycle(&LED, 0);
}
PWM_SetDutyCycle(&LED, duty);
return;
}
void ESC_SetSpeed(ESC_t *esc)
{
PWM_SetDutyCycle(esc->pin_number, esc->run_min + (uint16_t)(esc->speed * (esc->run_max - esc->run_min)));
/*switch(esc->pin_number){
case 12:
TIM4->CCR1 = esc->run_min + (uint16_t)(esc->speed * (esc->run_max - esc->run_min));
break;
case 13:
TIM4->CCR2 = esc->run_min + (uint16_t)(esc->speed * (esc->run_max - esc->run_min));
break;
case 14:
TIM4->CCR3 = esc->run_min + (uint16_t)(esc->speed * (esc->run_max - esc->run_min));
break;
case 15:
TIM4->CCR4 = esc->run_min + (uint16_t)(esc->speed * (esc->run_max - esc->run_min));
break;
default:
// do nothing
break;
}*/
}
uint8_t rightR2Motor(char speed) {
dbprintf("Entered %s with speed %d\n", __FUNCTION__, speed);
unsigned int newSpeed;
if (speed > 100 || speed < -100)
return ERROR;
//Cap the Duty Cycle at 60% for now
if (speed > 60)
speed = 60;
if (speed < -60)
speed = -60;
if (speed < 0) {
dbprintf("%s: Set to backwards\n", __FUNCTION__);
RIGHT_MOTOR_DIR = BACK;
newSpeed = speed * (-1); //Set as positive value
} else {
dbprintf("%s: Set to Forward\n", __FUNCTION__);
RIGHT_MOTOR_DIR = FORWARD;
newSpeed = speed;
}
//set the speed
if ((PWM_SetDutyCycle(RIGHT_MOTOR_ENABLE, newSpeed * 10)) != SUCCESS) {
return ERROR;
}
return SUCCESS;
}
static char Right_MtrSpeed(int speed)
{
// Check argument range
if (speed > MAX_PWM || speed < ((-1) * MAX_PWM)) {
return ERROR;
}
// Check direction
if (speed < 0) {
// Reverse
speed = speed * (-1);
// Clear direction
IO_PortsClearPortBits(MOTOR_PINS_PORT, MOTOR_PINS_RIGHT_DIR);
} else {
// Forward
// Set direction
IO_PortsSetPortBits(MOTOR_PINS_PORT, MOTOR_PINS_RIGHT_DIR);
}
// Set PWM
speed = ConvertDC(speed);
PWM_SetDutyCycle(MOTOR_PINS_RIGHT_EN, speed);
return SUCCESS;
}
static char Left_MtrSpeed(int speed)
{
// Check argument range
if (speed > MAX_PWM || speed < ((-1) * MAX_PWM)) {
return ERROR;
}
// Modify speed for ratio
speed = (int) ((float) speed * MOTOR_RATIO);
// Check direction
if (speed < 0) {
// Reverse
speed = speed * (-1);
// Clear direction
IO_PortsClearPortBits(MOTOR_PINS_PORT, MOTOR_PINS_LEFT_DIR);
} else {
// Forward
// Set direction
IO_PortsSetPortBits(MOTOR_PINS_PORT, MOTOR_PINS_LEFT_DIR);
}
// Set PWM
speed = ConvertDC(speed);
PWM_SetDutyCycle(MOTOR_PINS_LEFT_EN, speed);
return SUCCESS;
}
void MOTOR_Start(uint32_t motor, uint32_t speed) {
int j = 0;
if(motor == MOTOR_RIGHT) {
PWM_Start(PWM1);
for(j = 0 ; j < 1000 ; j++) { }
PWM_SetDutyCycle(PWM1, speed);
} else if(motor == MOTOR_LEFT) {
PWM_Start(PWM2);
for(j = 0 ; j < 1000 ; j++) { }
PWM_SetDutyCycle(PWM2, speed);
}
}
void MOTOR_Start(Motor motor, uint32_t speed) {
int j = 0;
if(motor == RIGHT_MOTOR) {
PWM_Start(PWM1);
for(j = 0 ; j < 1000 ; j++) { }
PWM_SetDutyCycle(PWM1, speed);
} else if(motor == LEFT_MOTOR) {
PWM_Start(PWM2);
for(j = 0 ; j < 1000 ; j++) { }
PWM_SetDutyCycle(PWM2, speed);
}
}
int16_t motor_command(int16_t command){
PWM_SetDutyCycle(PWM_PORTY04, abs_val(command));
if(command>0){
DIRECTION = FORWARD;
}else{
DIRECTION = REVERSE;
}
return 0;
}
uint8_t R2LauncherMotorSpeed(char speed) {
dbprintf("Entered %s with speed %d\n", __FUNCTION__, speed);
unsigned int newSpeed;
if (speed > 100 || speed < 0)
return ERROR;
newSpeed = speed;
//Cap the Duty Cycle at 33% for now
if (speed > 33) newSpeed = 33;
//set the speed
if ((PWM_SetDutyCycle(LAUNCHER_MOTOR_ENABLE, newSpeed * 10)) != SUCCESS) {
return ERROR;
}
return SUCCESS;
}
/***********************************************************
* Function ： PWM_InitPwmMode
* Parameter ：
* Return ：
* Description ：
***********************************************************/
void PWM_InitPwmMode(PWM_PWM_CFG *pPwmPara)
{
/*disable INTSRC_PWM*/
VIC_DisableInt(INTSRC_PWM);
PWM_Enable(pPwmPara->PwmId, FALSE);
/*set pwm's mode*/
PWM_SetMode(pPwmPara->PwmId, MODE_PWM);
/*set PWM prescale*/
PWM_SetPreScale(pPwmPara->PwmId, pPwmPara->Prescale);
/*set period register*/
PWM_SetPeriod(pPwmPara->PwmId, pPwmPara->Period);
/*set duty cycle*/
PWM_SetDutyCycle(pPwmPara->PwmId, pPwmPara->DutyCycle);
if(pPwmPara->PwmMask == INT_UNMASK)
{
/*configure int for pwm*/
PWM_MaskInt(pPwmPara->PwmId, pPwmPara->PwmMask);
VIC_EnableInt(INTSRC_PWM);
}
PWM_Enable(pPwmPara->PwmId, TRUE);
}
static void prvPWMSetTask (void* pvParameters)
{
static uint8_t counter=0;
static uint8_t gaz=1;
portTickType xLastWakeTime;
xLastWakeTime=xTaskGetTickCount();
uint8_t percent=0;
while (1)
{
percent=0;
if (counter<60) counter++;
else if ((counter<69
+6))
{
percent=/*0;//*/gaz*10;
gaz+=1;
PWM_SetDutyCycle(percent);
counter++;
}
else if (counter<90 )
{
counter++;
}
//gege szerint ez igy jo lesz
else PWM_SetDutyCycle(0);
//bluetooth
/*
switch (percent/10)
{
case 0 :
xQueueSend(TransmitQueue,'0',( portTickType )0);
break;
case 1 :
xQueueSend(TransmitQueue,'1',( portTickType )0);
break;
case 2 :
xQueueSend(TransmitQueue,'2',( portTickType )0);
break;
case 3 :
xQueueSend(TransmitQueue,'3',( portTickType )0);
break;
case 4 :
xQueueSend(TransmitQueue,'4',( portTickType )0);
break;
case 5 :
xQueueSend(TransmitQueue,'5',( portTickType )0);
break;
case 6 :
xQueueSend(TransmitQueue,'6',( portTickType )0);
break;
case 7 :
xQueueSend(TransmitQueue,'7',( portTickType )0);
break;
case 8 :
xQueueSend(TransmitQueue,'8',( portTickType )0);
break;
case 9 :
xQueueSend(TransmitQueue,'9',( portTickType )0);
break;
case 10 :
xQueueSend(TransmitQueue,'1',( portTickType )0);
xQueueSend(TransmitQueue,'0',( portTickType )0);
break;
}
xQueueSend(TransmitQueue,' ',( portTickType )0);
UARTStartSend();
*/
vTaskDelayUntil(&xLastWakeTime,1000);
}
}
