blob: 3d7bbce4c994c86b20741a16456a6b38b7b5d102 [file] [log] [blame]
/*
* Copyright (c) 2017 FH Dortmund.
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the Eclipse Public License v1.0
* which accompanies this distribution, and is available at
* http://www.eclipse.org/legal/epl-v10.html
*
* Description:
* A4MCAR Project - Low-level Module Servo Motor driver task for steering
*
* Authors:
* M. Ozcelikors <mozcelikors@gmail.com>
*
* Update History:
*
*/
#include "servo.h"
#include "core_debug.h"
/***
* Function Name: Task_ApplyPWMTo1BitPort
* Function Description : This function can be used to apply PWM signal to a 1-bit port.
* The default period can be changed by the variable overall_pwm_period,
* keeping in mind that 100MHz clock results that 100000 timer ticks,
* which means 1 millisecond.
*
* Argument Type Description
* p port 1-bit port to have pwm output
* duty_cycle int duty cycle percentage (0-100)
*/
// The following function is not used in our application, but is created as a pwm reference
void Task_ApplyPWMTo1BitPort (port p, int duty_cycle)
{
uint32_t overall_pwm_period = 20 * MILLISECOND;
uint32_t on_period = (duty_cycle * overall_pwm_period * (1.0/100));
uint32_t off_period = overall_pwm_period - on_period;
//Uncomment to following to debug calculated periods
//debug_printf("%d %d", on_period, off_period);
uint32_t time;
int port_state = 0;
timer tmr;
tmr :> time;
while(1)
{
select
{
case tmr when timerafter(time) :> void :
//Port toggling to create PWM output
if(port_state == 0)
{
p <: 1;
port_state = 1;
time += on_period; //Extend timer deadline
}
else if(port_state == 1)
{
p <: 0;
port_state = 0;
time += off_period; //Extend timer deadline
}
break;
}
}
}
/***
* Function Name: Task_SteeringServo_MotorController
* Function Description : Generates the appropriote PWM to the servo to drive it.
*
* Argument Type Description
* p out port 1-bit port to have pwm output
* steering_interface steering_if Steering of the A4MCAR: 0-> MOST RIGHT 50-> MIDDLE 99-> MOST LEFT
*/
[[combinable]]
void Task_SteeringServo_MotorController (out port p, server steering_if steering_interface)
{
uint32_t overall_pwm_period = STEERINGSERVO_PWM_PERIOD;
uint32_t on_period;
uint32_t off_period;
uint32_t time;
int port_state = 0;
timer tmr;
int steering;
PrintCoreAndTileInformation("Task_SteeringServo_MotorController");
// Timing measurement/debugging related definitions
timer debug_timer;
uint32_t start_time, end_time;
while (1)
{
select
{
//Wait for the steering value
case steering_interface.ShareSteeringValue (int steering_val):
steering = steering_val;
break;
//Calculate PWM periods and apply period within the timer
case tmr when timerafter(time) :> void :
//Measure start time
////debug_timer :> start_time;
tmr :> time;
if (steering == 0)
{
on_period = STEERINGSERVO_PWM_MAXRIGHT_PULSE_WIDTH;
}
else if (steering > 100)
{
on_period = STEERINGSERVO_PWM_MAXLEFT_PULSE_WIDTH;
}
else
{
on_period = (STEERINGSERVO_PWM_MAXRIGHT_PULSE_WIDTH - ((STEERINGSERVO_PWM_MAXRIGHT_PULSE_WIDTH - STEERINGSERVO_PWM_MAXLEFT_PULSE_WIDTH) * (steering/100.0)));
}
off_period = overall_pwm_period - on_period;
//Port Toggling for PWM output
if(port_state == 0)
{
p <: 1;
port_state = 1;
time += on_period; //Extend timer deadline
}
else if(port_state == 1)
{
p <: 0;
port_state = 0;
time += off_period; //Extend timer deadline
}
//Measure end time
////debug_timer :> end_time;
////printf("SERVO t: %u", end_time - start_time);
break;
}
}
}