From: Nathael Pajani Date: Tue, 26 Apr 2016 22:55:52 +0000 (+0200) Subject: Servo-motor control example X-Git-Url: http://git.techno-innov.fr/?a=commitdiff_plain;h=f8141a9e1cb4862fb94b48c04229ed75694eaffd;p=soft%2Flpc122x%2Fexamples Servo-motor control example --- diff --git a/servomotor/Makefile b/servomotor/Makefile new file mode 100644 index 0000000..954c63f --- /dev/null +++ b/servomotor/Makefile @@ -0,0 +1,13 @@ +# Makefile for "base" apps +# This includes apps for the GPIO Demo Module and for the LPC1224-BO board. + +MODULE = $(shell basename $(shell cd .. && pwd && cd -)) +NAME = $(shell basename $(CURDIR)) + +.PHONY: $(NAME).bin +$(NAME).bin: + @make -C ../../.. --no-print-directory NAME=$(NAME) MODULE=$(MODULE) apps/$(MODULE)/$(NAME)/$@ + +clean mrproper: + @make -C ../../.. --no-print-directory $@ + diff --git a/servomotor/README b/servomotor/README new file mode 100644 index 0000000..2ebfd0e --- /dev/null +++ b/servomotor/README @@ -0,0 +1,33 @@ +Servo-motor control using ADC input for angle control + +Copyright 2016 Nathael Pajani + + +/* **************************************************************************** + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + *************************************************************************** */ + +This example shows the support of PWM for servo-motor control. +Servo-motor position is set by reading on ADC input 0. + +The PWM signal is output on port 0 pin 19 (LPC_TIMER_32B0_M1_PIO_0_19), using +32 bits timer 0, match channel 1. + +The duty cycle is defined at the beginning of the code using "DUTY_INVERTED". +Set to "1" to get an inverted duty-cycle signal, for use with a single transistor +or mosfet. +Set to "0" to get a non-inverted signal, for use with two transistors or mosfets, +or with none. + diff --git a/servomotor/main.c b/servomotor/main.c new file mode 100644 index 0000000..61ef626 --- /dev/null +++ b/servomotor/main.c @@ -0,0 +1,210 @@ +/**************************************************************************** + * apps/base/servo/main.c + * + * Servo-motor example + * + * Copyright 2016 Nathael Pajani + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + *************************************************************************** */ + + +#include +#include "core/lpc_regs_12xx.h" +#include "core/lpc_core_cm0.h" +#include "core/pio.h" +#include "core/system.h" +#include "core/systick.h" +#include "lib/stdio.h" +#include "drivers/serial.h" +#include "drivers/gpio.h" +#include "extdrv/status_led.h" +#include "drivers/adc.h" +#include "drivers/timers.h" + + +#define MODULE_VERSION 0x04 +#define MODULE_NAME "GPIO Demo Module" + + +#define SELECTED_FREQ FREQ_SEL_48MHz + +/* Chose on of these depending on the signal you need: + * - inverted one (3), for use with a single transistor + * - non inverted (40 - 3), for use with two transistors (or none). + */ +#define DUTY_INVERTED 1 +#if (DUTY_INVERTED == 1) +#define SERVO_MED_POS_DUTY_CYCLE 3 +#else +#define SERVO_MED_POS_DUTY_CYCLE (40 - 3) +#endif + +#define LPC_TIMER_PIN_CONFIG (LPC_IO_MODE_PULL_UP | LPC_IO_DIGITAL | LPC_IO_DRIVE_HIGHCURENT) + +/***************************************************************************** */ +/* Pins configuration */ +/* pins blocks are passed to set_pins() for pins configuration. + * Unused pin blocks can be removed safely with the corresponding set_pins() call + * All pins blocks may be safelly merged in a single block for single set_pins() call.. + */ +const struct pio_config common_pins[] = { + /* UART 0 */ + { LPC_UART0_RX_PIO_0_1, LPC_IO_DIGITAL }, + { LPC_UART0_TX_PIO_0_2, LPC_IO_DIGITAL }, + /* TIMER_32B0 */ + { LPC_TIMER_32B0_M1_PIO_0_19, LPC_TIMER_PIN_CONFIG }, + ARRAY_LAST_PIO, +}; + +const struct pio_config adc_pins[] = { + { LPC_ADC_AD0_PIO_0_30, LPC_IO_ANALOG }, + { LPC_ADC_AD1_PIO_0_31, LPC_IO_ANALOG }, + { LPC_ADC_AD2_PIO_1_0, LPC_IO_ANALOG }, + { LPC_ADC_AD3_PIO_1_1, LPC_IO_ANALOG }, + { LPC_ADC_AD4_PIO_1_2, LPC_IO_ANALOG }, + { LPC_ADC_AD5_PIO_1_3, LPC_IO_ANALOG }, + ARRAY_LAST_PIO, +}; + +const struct pio status_led_green = LPC_GPIO_1_4; +const struct pio status_led_red = LPC_GPIO_1_5; + +/***************************************************************************** */ +static uint32_t servo_med_pos_cmd = 0; +static uint32_t servo_one_deg_step = 0; +static uint8_t timer = 0; +static uint8_t channel = 0; +int servo_config(uint8_t timer_num, uint8_t pwm_chan, uint8_t uart_num) +{ + uint32_t servo_command_period = 0; + struct timer_config timer_conf = { + .mode = LPC_TIMER_MODE_PWM, + .config = { 0, 3, 0, 0 }, /* Use channel 3 for control */ + }; + + if (timer_num > LPC_TIMER_32B1) { + uprintf(uart_num, "Bad timer number\n"); + return -1; + } + if (pwm_chan >= 3) { + uprintf(uart_num, "Bad channel number\n"); + return -1; + } + timer = timer_num; + channel = pwm_chan; + + /* compute the period and median position for the servo command */ + /* We want 20ms (50Hz), timer counts at main clock frequency */ + servo_command_period = get_main_clock() / 50; + /* servo_command_period is 20ms, we need 1.5ms, which is 3/40. */ + servo_med_pos_cmd = ((servo_command_period / 40) * SERVO_MED_POS_DUTY_CYCLE); + servo_one_deg_step = ((servo_command_period / 41) / 48); + timer_conf.match[pwm_chan] = servo_med_pos_cmd; + timer_conf.match[3] = servo_command_period; + timer_conf.config[0] = (LPC_PWM_CHANNEL_ENABLE(pwm_chan)); + + timer_on(timer, 0, NULL); + timer_setup(timer, &timer_conf); + timer_start(timer); + + uprintf(uart_num, "Servos configured (T%d : C%d), Period : %d, med_pos : %d\n", + timer, channel, servo_command_period, servo_med_pos_cmd); + + return 0; +} + +int set_servo(int adc_num, int uart_num) +{ + uint16_t val = 0, angle = 0; + uint32_t pos = servo_med_pos_cmd; + + adc_start_convertion_once(adc_num, 0); + msleep(10); + adc_get_value(&val, adc_num); + uprintf(uart_num, "ADC(%d): %d (raw: 0x%04x)\n", adc_num, val, val); + + angle = ((val * 10) / 56); + if (angle > 180) { + angle = 180; + } + + /* And compute the new match value for the angle */ + if (angle >= 90) { + pos += ((angle - 90) * servo_one_deg_step); + } else { + pos -= ((90 - angle) * servo_one_deg_step); + } + timer_set_match(timer, channel, pos); + uprintf(uart_num, "Servo(%d): %d (%d)\n", channel, angle, pos); + return val; +} + + +/***************************************************************************** */ +void system_init() +{ + /* Stop the watchdog */ + startup_watchdog_disable(); /* Do it right now, before it gets a chance to break in */ + + /* Note: Brown-Out detection must be powered to operate the ADC. adc_on() will power + * it back on if called after system_init() */ + system_brown_out_detection_config(0); + system_set_default_power_state(); + clock_config(SELECTED_FREQ); + set_pins(common_pins); + set_pins(adc_pins); + gpio_on(); + status_led_config(&status_led_green, &status_led_red); + /* System tick timer MUST be configured and running in order to use the sleeping + * functions */ + systick_timer_on(1); /* 1ms */ + systick_start(); +} + +/* Define our fault handler. This one is not mandatory, the dummy fault handler + * will be used when it's not overridden here. + * Note : The default one does a simple infinite loop. If the watchdog is deactivated + * the system will hang. + */ +void fault_info(const char* name, uint32_t len) +{ + serial_write(0, name, len); + /* Wait for end of Tx */ + serial_flush(0); + /* FIXME : Perform soft reset of the micro-controller ! */ + while (1); +} + + + +/***************************************************************************** */ +int main(void) { + system_init(); + uart_on(0, 115200, NULL); + adc_on(); + servo_config(LPC_TIMER_32B0, 1, 0); + + while (1) { + chenillard(500); + /* ADC Test */ + set_servo(LPC_ADC_NUM(0), 0); + } + return 0; +} + + +