void servo_init(Leg *legs){
//Set up the servos using the leg details + status LEDs
volatile uint8_t *ports[PWM_COUNT];
uint8_t pins[PWM_COUNT];
for (uint8_t l = 0; l < LEG_COUNT; l++){
for (uint8_t j = 0; j < JOINT_COUNT; j++){
ports[(l * JOINT_COUNT) + j] = legs[l].getPort(j);
pins[(l * JOINT_COUNT) + j] = legs[l].getPin(j);
}
for (uint8_t j = 0; j < CALIBRATION_COUNT; j++){
legs[l].setCalibration(j, eeprom_read_byte((uint8_t*) (l * CALIBRATION_COUNT) + j));
}
}
status_init(ports, pins);
pwm_init(ports, pins, PWM_COUNT, 20000);
status_set_color(0x00, 0x00, 0x00);
for (uint8_t l = 0; l < LEG_COUNT; l++){
legs[l].resetPosition();
}
pwm_apply_batch();
delay_ms(500);
pwm_stop();
}
static void pwm_handler(struct work_struct *w)
{
struct pwm_device *p = container_of(w, struct pwm_device,
handler_work);
if (p->handler && p->handler(p, p->handler_data))
pwm_stop(p);
}
void moveDistance(int distance)
{
motorInit();
// Start PWM process. Period 1 ms, Freq 1 kHz
pwm_start(basepwm);
// Turn motors counterclockwise for 3 s.
if(distance >=0){
high(M1forward);
high(M2forward);
} else {
high(M1reverse);
high(M2reverse);
distance=abs(distance);
}
pwm_set(M1enable, 0, 1000);
pwm_set(M2enable, 1, 1000);
pause(distance);
// Stop again
pwm_set(M1enable, 0, 0);
pwm_set(M2enable, 1, 0);
// End the PWM process
pwm_stop();
}
static int __init mini2451_pwm_dev_init(void) {
int ret;
ret = gpio_request(BUZZER_PMW_GPIO, DEVICE_NAME);
if (ret) {
printk("request GPIO %d for pwm failed\n", BUZZER_PMW_GPIO);
return ret;
}
gpio_set_value(BUZZER_PMW_GPIO, 0);
s3c_gpio_cfgpin(BUZZER_PMW_GPIO, S3C_GPIO_OUTPUT);
pwm4buzzer = pwm_request(BUZZER_PWM_ID, DEVICE_NAME);
if (IS_ERR(pwm4buzzer)) {
printk("request pwm %d for %s failed\n", BUZZER_PWM_ID, DEVICE_NAME);
return -ENODEV;
}
pwm_stop();
sema_init(&lock, 1);
ret = misc_register(&mini2451_misc_dev);
printk(DEVICE_NAME "\tinitialized\n");
return ret;
}
void moveMotorT(int left, int right, int time)
{
motorInit();
// Start PWM process. Period 1 ms, Freq 1 kHz
pwm_start(basepwm);
if(left >=0){
high(M1forward);
} else {
high(M1reverse);
left=abs(left);
}
if(right >=0){
high(M2forward);
} else {
high(M2reverse);
right=abs(right);
}
pwm_set(M1enable, 0, left);
pwm_set(M2enable, 1, right);
pause(time);
// Stop again
pwm_set(M1enable, 0, 0);
pwm_set(M2enable, 1, 0);
// End the PWM process
pwm_stop();
}
static int pwm_backlight_update_status(struct backlight_device *bl)
{
struct pwm_bl_data *pb = dev_get_drvdata(&bl->dev);
int brightness = bl->props.brightness;
int max = bl->props.max_brightness;
if (bl->props.power != FB_BLANK_UNBLANK)
brightness = 0;
if (bl->props.fb_blank != FB_BLANK_UNBLANK)
brightness = 0;
if (pb->notify)
brightness = pb->notify(pb->dev, brightness);
pwm_set_period_ns(pb->pwm, pb->period);
if (brightness == 0) {
pwm_set_duty_ns(pb->pwm, 0);
if(pb->pwm_started != 0){
pwm_stop(pb->pwm);
pb->pwm_started = 0;
}
} else {
pwm_set_duty_ns(pb->pwm, brightness * pb->period / max);
if(pb->pwm_started == 0){
pwm_start(pb->pwm);
pb->pwm_started = 1;
}
}
return 0;
}
void pwm_restart()
{
if (pwmInfo.running)
{
pwm_stop();
pwm_start();
}
}
void KeyScan_PlayToneStop()
{
if(!ubIsPlaying)
return;
pwm_stop(PWMID_0);
ubIsPlaying = FALSE;
}
/* IRQ handler */
static int irq_steps_handler (struct pwm_channel *ch, void *data) {
int id = pwmc_to_id (ch);
steps[id]++;
if (steps[id] >= steps_max[id])
pwm_stop(ch);
return 0;
}
void stopMotors()
{
motorInit();
// Stop
pwm_set(M1enable, 0, 0);
pwm_set(M2enable, 1, 0);
// End the PWM process
pwm_stop();
}
void adc_stop(void)
{
TRACE_INFO("ADC STOP\n");
pio_stop();
gAdcDesc.buf_wr=gAdcDesc.buf_rd=0;
gAdcDesc.cb=NULL;
gAdcDesc.state=ADC_IDLE;
#if PWM_DEBUG
pwm_stop();
#endif
}
void stop_moving(void)
{
pwm_stop();
CHIP_DISABLE();
/* next start request runs in opposite direction */
if (direction == UP) {
direction = DOWN;
} else {
direction = UP;
}
}
void servo_init()
{
int i;
for(i=0;i<sizeof(servo_pin)/sizeof(servo_pin[0]);i++)
{
pwm_stop(servo_pin[i][0], servo_pin[i][1]);
pwm_set_period(servo_pin[i][0], servo_pin[i][1], SERVO_STD_PERIOD);
pwm_set_polarity(servo_pin[i][0], servo_pin[i][1], 1);
pwm_set_duty_cycle(servo_pin[i][0], servo_pin[i][1], SERVO_ANGLE_TO_DUTY(0)); /* keep in the middle */
}
start_servo();
}
void play(char m)
{
if (m == ' ') {
tmr_delay(tmrid, 2 * dot_delay);
} else {
pio_pin_sethigh("PF_0");
pwm_start(pwmid);
tmr_delay(tmrid, m == '.' ? dot_delay : 3 *dot_delay);
pwm_stop(pwmid);
pio_pin_setlow("PF_0");
tmr_delay(tmrid, dot_delay);
}
}
void pwm_update(uint16_t position, int32_t pwm)
// Update the PWM signal being sent to the motor. The PWM value should be
// a signed integer in the range of -65535 to -1 for clockwise movement,
// 1 to 65535 for counter-clockwise movement or zero to stop all movement.
// This function provides a sanity check against the servo position and
// will prevent the servo from being driven past a minimum and maximum
// position.
{
uint16_t pwm_width;
// Determine if PWM is disabled in the registers.
if (!(registers_read_byte(REG_FLAGS_LO) & (1<<FLAGS_LO_PWM_ENABLED))) pwm = 0;
// Determine direction of servo movement or stop.
if (pwm < 0)
{
// Less than zero. Turn clockwise.
// Get the PWM width from the PWM value.
pwm_width = (uint16_t) -pwm;
// Turn clockwise.
#if SWAP_PWM_DIRECTION_ENABLED
pwm_dir_a(pwm_width);
#else
pwm_dir_b(pwm_width);
#endif
}
else if (pwm > 0)
{
// More than zero. Turn counter-clockwise.
// Get the PWM width from the PWM value.
pwm_width = (uint16_t) pwm;
// Turn counter-clockwise.
#if SWAP_PWM_DIRECTION_ENABLED
pwm_dir_b(pwm_width);
#else
pwm_dir_a(pwm_width);
#endif
}
else
{
// Stop all PWM activity to the motor.
pwm_stop();
}
}
pwm_result_t pwm_disable(pwm_handle handle)
{
pwm_internal *pwm;
struct pwm_channel *p;
pwm_result_t ret = PWM_OK;
pwm = (pwm_internal *)handle;
if (!pwm || (PWM_MAGIC !=pwm->magic) || !pwm->data)
{
return PWM_INVALID_PARAM;
}
p = (struct pwm_channel *)pwm->data;
if (pwm_stop(p)) ret = PWM_INVALID_PARAM;
return ret;
}
static int pwm_backlight_suspend(struct platform_device *pdev,
pm_message_t state)
{
struct backlight_device *bl = platform_get_drvdata(pdev);
struct pwm_bl_data *pb = dev_get_drvdata(&bl->dev);
if (pb->notify)
pb->notify(pb->dev, 0);
pwm_set_duty_ns(pb->pwm, 0);
if(pb->pwm_started != 0){
pwm_stop(pb->pwm);
pb->pwm_started = 0;
}
return 0;
}
static int pwm_backlight_remove(struct platform_device *pdev)
{
struct platform_pwm_backlight_data *data = pdev->dev.platform_data;
struct backlight_device *bl = platform_get_drvdata(pdev);
struct pwm_bl_data *pb = dev_get_drvdata(&bl->dev);
backlight_device_unregister(bl);
pwm_set_duty_ns(pb->pwm, 0);
pwm_stop(pb->pwm);
pwm_release(pb->pwm);
kfree(pb);
if (data->exit)
data->exit(&pdev->dev);
return 0;
}
/**
Turn on LED
Turn on LED.
@param UINT32 uiLED: Which LED, could be the following
GPIOMAP_LED_GREEN
GPIOMAP_LED_RED
GPIOMAP_LED_FCS //FOCUS
@return void
*/
void LED_TurnOnLED(UINT32 uiLED)
{
DBG_IND("LED_TurnOnLED %d\r\n",uiLED);
switch (uiLED)
{
case GPIOMAP_LED_GREEN:
#if (LED_GREEN_CTRL == LED_GREEN_BY_GPIO)
gpio_setPin(GPIO_GREEN_LED);
#endif
break;
case GPIOMAP_LED_RED:
#if (LED_RED_CTRL == LED_RED_BY_GPIO)
gpio_setPin(GPIO_RED_LED);
#endif
break;
case GPIOMAP_LED_FCS:
#if (LED_FOCUS_CTRL == LED_FOCUS_BY_GPIO)
gpio_setPin(GPIO_FOCUS_LED);
#endif
#if (LED_FOCUS_CTRL == LED_FOCUS_BY_PWM)
{
//set FOCUS LED brightness level
PWM_CFG PWMInfo;
if(pwm_open(PWMID_FOCUS_LED)!=E_OK)
{
pwm_stop(PWMID_FOCUS_LED);
pwm_close(PWMID_FOCUS_LED, TRUE);
pwm_open(PWMID_FOCUS_LED);
}
PWMInfo.uiDiv=220;
PWMInfo.uiPrd=22;
PWMInfo.uiFall=FcsLedPrdVal[FcsLedLvl];
PWMInfo.uiOnCycle=0;
PWMInfo.uiInv=0;
PWMInfo.uiRise=0;
pwm_set(PWMID_FOCUS_LED, &PWMInfo);
pwm_reload(PWMID_FOCUS_LED);
}
#endif
default:
break;
}
}
void vibtonz_en(bool en)
{
t_vib_desc *vib_iter =&vib_desc;
printk("%s %s \n", __func__, (en?"enabled":"disabled"));
if( vib_iter->initialized == 0) return;
if(en)
{
vib_iter->gpio_en(en);
pwm_start(vib_iter->pwm);
}
else
{
pwm_stop(vib_iter->pwm);
vib_iter->gpio_en(en);
}
}
static long mini2451_pwm_ioctl(struct file *filep, unsigned int cmd,
unsigned long arg)
{
switch (cmd) {
case PWM_IOCTL_SET_FREQ:
if (arg == 0)
return -EINVAL;
pwm_set_freq(arg);
break;
case PWM_IOCTL_STOP:
default:
pwm_stop();
break;
}
return 0;
}
static ssize_t pwm_run_store(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t len)
{
struct pwm_device *p = dev_get_drvdata(dev);
int ret;
if (sysfs_streq(buf, "1"))
ret = pwm_start(p);
else if (sysfs_streq(buf, "0"))
ret = pwm_stop(p);
else
ret = -EINVAL;
if (ret < 0)
return ret;
return len;
}
void pwm_set_freq(uint16_t freq)
{
pwmInfo.freq = freq;
/* Stop now to avoid load being used */
if (pwmInfo.running)
{
pwm_stop();
pwmInfo.running = 1;
}
timer_set_frequency(FRC1, freq);
pwmInfo._maxLoad = timer_get_load(FRC1);
if (pwmInfo.running)
{
pwm_start();
}
}
void pwm_set_duty(uint16_t duty)
{
bool output;
pwmInfo.dutyCicle = duty;
if (duty > 0 && duty < UINT16_MAX) {
pwm_restart();
return;
}
// 0% and 100% duty cycle are special cases: constant output.
pwm_stop();
pwmInfo.running = 1;
output = (duty == UINT16_MAX);
for (uint8_t i = 0; i < pwmInfo.usedPins; ++i)
{
gpio_write(pwmInfo.pins[i].pin, output);
}
}
static int ehrpwm_freq_transition_cb(struct pwm_device *p)
{
struct ehrpwm_pwm *ehrpwm = to_ehrpwm_pwm(p);
unsigned long duty_ns;
p->tick_hz = clk_get_rate(ehrpwm->clk);
duty_ns = p->duty_ns;
if (pwm_is_running(p)) {
pwm_stop(p);
pwm_set_duty_ns(p, 0);
pwm_set_period_ns(p, p->period_ns);
pwm_set_duty_ns(p, duty_ns);
pwm_start(p);
} else {
pwm_set_duty_ns(p, 0);
pwm_set_period_ns(p, p->period_ns);
pwm_set_duty_ns(p, duty_ns);
}
return 0;
}
void motorTest()
{
motorInit();
// Start PWM process. Period 1 ms, Freq 1 kHz
pwm_start(basepwm);
// Turn motors counterclockwise for 3 s.
high(M1forward);
high(M2forward);
pwm_set(M1enable, 0, 1000);
pwm_set(M2enable, 1, 1000);
pause(2000);
// Stop again
pwm_set(M1enable, 0, 0);
pwm_set(M2enable, 1, 0);
// End the PWM process
pwm_stop();
}
void pwm_release(struct pwm_device *p)
{
mutex_lock(&device_list_mutex);
if (!test_and_clear_bit(FLAG_REQUESTED, &p->flags)) {
pr_debug("%s pwm device is not requested!\n",
dev_name(p->dev));
goto done;
}
pwm_stop(p);
pwm_unsynchronize(p, NULL);
pwm_set_handler(p, NULL, NULL);
p->label = NULL;
p->pid = -1;
if (p->ops->release)
p->ops->release(p);
done:
mutex_unlock(&device_list_mutex);
}
static int ecap_frequency_transition_cb(struct pwm_device *p)
{
struct ecap_pwm *ep = to_ecap_pwm(p);
unsigned long duty_ns, rate;
rate = clk_get_rate(ep->clk);
if (rate == p->tick_hz)
return 0;
p->tick_hz = rate;
duty_ns = p->duty_ns;
if (pwm_is_running(p)) {
pwm_stop(p);
pwm_set_duty_ns(p, 0);
pwm_set_period_ns(p, p->period_ns);
pwm_set_duty_ns(p, duty_ns);
pwm_start(p);
} else {
pwm_set_duty_ns(p, 0);
pwm_set_period_ns(p, p->period_ns);
pwm_set_duty_ns(p, duty_ns);
}
return 0;
}
void pwm_init(uint8_t npins, uint8_t* pins)
{
/* Assert number of pins is correct */
if (npins > MAX_PWM_PINS)
{
printf("Incorrect number of PWM pins (%d)\n", npins);
return;
}
/* Initialize */
pwmInfo._maxLoad = 0;
pwmInfo._onLoad = 0;
pwmInfo._offLoad = 0;
pwmInfo._step = PERIOD_ON;
/* Save pins information */
pwmInfo.usedPins = npins;
uint8_t i = 0;
for (; i < npins; ++i)
{
pwmInfo.pins[i].pin = pins[i];
/* configure GPIOs */
gpio_enable(pins[i], GPIO_OUTPUT);
}
/* Stop timers and mask interrupts */
pwm_stop();
/* set up ISRs */
_xt_isr_attach(INUM_TIMER_FRC1, frc1_interrupt_handler);
/* Flag not running */
pwmInfo.running = 0;
}
static void __exit mini2451_pwm_dev_exit(void) {
pwm_stop();
misc_deregister(&mini2451_misc_dev);
gpio_free(BUZZER_PMW_GPIO);
}
