| 0,0 → 1,163 |
|---|
| |
| /* |
| * Project: S.Ha.R.K. |
| * |
| * Coordinators: Giorgio Buttazzo <giorgio@sssup.it> |
| * |
| * Authors : Paolo Gai <pj@hartik.sssup.it> |
| * (see authors.txt for full list of hartik's authors) |
| * |
| * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
| * |
| * http://www.sssup.it |
| * http://retis.sssup.it |
| * http://shark.sssup.it |
| */ |
| |
| /* |
| * Copyright (C) 2000 Paolo Gai |
| * |
| * 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, write to the Free Software |
| * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
| * |
| */ |
| |
| #include "servo.h" |
| #include <kernel/kern.h> |
| #include <drivers/scom.h> |
| #include <drivers/keyb.h> |
| |
| /* COM Port Constants */ |
| #define COM_PORT COM2 |
| #define COM_SPEED 115200 |
| #define COM_PARITY NONE |
| #define COM_LEN 8 |
| #define COM_STOP 1 |
| |
| /* Real Servo Value */ |
| #define TICK_ZERO 1600 |
| #define TICK_ANG (1800/180.0) |
| #define TICK_LEN 1.6 |
| #define TICK_AL ((float) TICK_ANG / TICK_LEN) |
| |
| int init_serial() |
| { |
| int err=0; |
| err = servo_open(COM_PORT, COM_SPEED); |
| //if (com_open(COM_PORT, COM_SPEED, COM_PARITY, COM_LEN, COM_STOP)==-1) err = 1; |
| |
| return err; |
| } |
| |
| void end_serial() |
| { |
| servo_close(COM_PORT); |
| //com_close(COM_PORT); |
| } |
| |
| TASK pic_generator() |
| { |
| int angle = -90; |
| int n_tick; |
| unsigned char buf, ech; |
| |
| while (1) { |
| |
| if (angle>90) angle = -90; |
| |
| servo_set_angle_sec(COM2, 0,ANGLE2SEC(angle,0,0)); |
| |
| /*n_tick = TICK_AL * (signed char)angle; |
| com_send(COM_PORT, 0); |
| ech = com_receive(COM_PORT); |
| |
| buf = n_tick / 256; |
| com_send(COM_PORT, buf); |
| ech = com_receive(COM_PORT); |
| |
| buf = n_tick % 256; |
| com_send(COM_PORT, buf); |
| ech = com_receive(COM_PORT);*/ |
| |
| |
| angle += 5; |
| |
| task_endcycle(); |
| } |
| return 0; |
| } |
| |
| |
| void endfun(KEY_EVT *k) |
| { |
| cprintf("Ctrl-Brk pressed! Ending...\n"); |
| sys_end(); |
| } |
| |
| void my_close(void *arg) |
| { |
| int i; |
| TIME tmp; |
| |
| for (i=3; i<MAX_PROC; i++){ |
| if (!jet_getstat(i, NULL, &tmp, NULL, NULL)) |
| kern_printf("Task Name : %s - Max Time : %d\n", proc_table[i].name, (int)tmp); |
| } |
| |
| end_serial(); |
| } |
| |
| int main(int argc, char **argv) |
| { |
| KEY_EVT k; |
| TIME seme; |
| SOFT_TASK_MODEL mp; |
| PID pid; |
| |
| k.flag = CNTR_BIT; |
| k.scan = KEY_C; |
| k.ascii = 'c'; |
| keyb_hook(k,endfun); |
| |
| k.flag = CNTL_BIT; |
| k.scan = KEY_C; |
| k.ascii = 'c'; |
| keyb_hook(k,endfun); |
| |
| seme = sys_gettime(NULL); |
| srand(seme); |
| |
| sys_atrunlevel(my_close, NULL, RUNLEVEL_BEFORE_EXIT); |
| |
| if (init_serial()) { |
| perror("Could not initialize serial port."); |
| sys_end(); |
| } |
| |
| soft_task_default_model(mp); |
| soft_task_def_level(mp,1); |
| soft_task_def_ctrl_jet(mp); |
| soft_task_def_met(mp,700); |
| soft_task_def_period(mp,10000); |
| //soft_task_def_aperiodic(mp); |
| soft_task_def_usemath(mp); |
| pid = task_create("PIC_Generator", pic_generator, &mp, NULL); |
| if (pid == NIL) { |
| perror("Could not create task <Joystick>"); |
| sys_end(); |
| } else |
| task_activate(pid); |
| |
| return 0; |
| } |