Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1283 → Rev 1284

/demos/trunk/servo/servo.c
File deleted
/demos/trunk/servo/time.c
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;
}
/demos/trunk/servo/load.c
0,0 → 1,52
#include "kernel/kern.h"
#include "drivers/keyb.h"
 
#include "servo.h"
 
#define N_SEND 100000
 
int main () {
 
TIME seme;
int res, i, err=0;
int rnd1, rnd2;
//char ch;
 
seme = sys_gettime(NULL);
srand(seme);
 
servo_open(COM2, 115200);
 
/*cprintf("servo_set_RS232_baudrate\n");
res = servo_set_RS232_baudrate(COM2, 9600);
cprintf("Res = %d\n",res);
 
cprintf("servo_get_RS232_baudrate\n");
res = servo_get_RS232_baudrate(COM2);
cprintf("Res = %d\n",res);
 
cprintf("servo_store_RS232_baudrate\n");
res = servo_store_RS232_baudrate(COM2);
cprintf("Res = %d\n\n",res);
 
ch = keyb_getch(BLOCK);*/
 
 
for (i=0; i<N_SEND; i++) {
 
rnd1 = rand()%16;
rnd2 = rand()%180 - 90;
res = servo_set_angle_sec(COM2, rnd1,ANGLE2SEC(rnd2,0,0));
cprintf("Pin = %2d - Angle = %3d - Res = %d\n",rnd1, rnd2, res);
if (res) err++;
 
}
cprintf("\nEvents = %d; Errors = %d\n",i, err);
 
 
servo_close(COM2);
 
return 0;
 
}
 
/demos/trunk/servo/initfile.c
57,7 → 57,7
struct multiboot_info *mb = (struct multiboot_info *)arg;
 
EDF_register_level(EDF_ENABLE_ALL);
//CBS_register_level(CBS_ENABLE_ALL, 0);
CBS_register_level(CBS_ENABLE_ALL, 0);
RR_register_level(RRTICK, RR_MAIN_YES, mb);
dummy_register_level();
 
74,7 → 74,7
 
HARTPORT_init();
 
//KEYB_init(NULL);
KEYB_init(NULL);
 
__call_main__(mb);
 
/demos/trunk/servo/ctrl.c
0,0 → 1,298
 
/*
* 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 "kernel/kern.h"
#include "drivers/keyb.h"
 
#include "servo.h"
 
int main () {
 
int res;
char ch;
 
servo_open(COM2, 115200);
 
/*cprintf("servo_set_RS232_baudrate\n");
res = servo_set_RS232_baudrate(COM2, 115200);
cprintf("Res = %d\n",res);*/
 
cprintf("servo_get_RS232_baudrate\n");
res = servo_get_RS232_baudrate(COM2);
cprintf("Res = %d\n",res);
 
/*cprintf("servo_store_RS232_baudrate\n");
res = servo_store_RS232_baudrate(COM2);
cprintf("Res = %d\n\n",res);*/
 
ch = keyb_getch(BLOCK);
 
 
cprintf("servo_set_period\n");
res = servo_set_period(COM2, 20000);
cprintf("Res = %d\n",res);
 
cprintf("servo_get_period\n");
res = servo_get_period(COM2);
cprintf("Res = %d\n",res);
 
/*cprintf("servo_store_period\n");
res = servo_store_period(COM2);
cprintf("Res = %d\n\n",res);*/
 
ch = keyb_getch(BLOCK);
 
 
cprintf("servo_get_setup_switch\n");
res = servo_get_setup_switch(COM2);
cprintf("Res = %d\n",res);
 
cprintf("servo_set_RC5_switch\n");
res = servo_set_RC5_switch(COM2, 1);
cprintf("Res = %d\n\n",res);
 
ch = keyb_getch(BLOCK);
 
 
cprintf("servo_set_levels\n");
res = servo_set_levels(COM2, 0x00,0x01);
cprintf("Res = %d\n",res);
 
cprintf("servo_get_levels\n");
res = servo_get_levels(COM2, 1);
cprintf("Res = %d\n\n",res);
 
ch = keyb_getch(BLOCK);
 
cprintf("servo_turn_off\n");
res = servo_turn_off(COM2, 1);
cprintf("Res = %d\n\n",res);
 
ch = keyb_getch(BLOCK);
 
cprintf("servo_turn_on\n");
res = servo_turn_on(COM2, 1);
cprintf("Res = %d\n\n",res);
 
ch = keyb_getch(BLOCK);
 
cprintf("servo_turn_off_all\n");
res = servo_turn_off_all(COM2);
cprintf("Res = %d\n\n",res);
 
ch = keyb_getch(BLOCK);
 
cprintf("servo_turn_on_all\n");
res = servo_turn_on_all(COM2);
cprintf("Res = %d\n\n",res);
 
ch = keyb_getch(BLOCK);
 
/*cprintf("servo_store_levels\n");
res = servo_store_levels(COM2);
cprintf("Res = %d\n\n",res);
 
ch = keyb_getch(BLOCK);*/
 
 
cprintf("servo_get_analog\n");
res = servo_get_analog(COM2, 0);
cprintf("Res = %d\n\n",res);
 
ch = keyb_getch(BLOCK);
 
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 0,ANGLE2SEC(45,0,0));
cprintf("Res = %d\n",res);
 
cprintf("servo_get_angle_sec\n");
res = servo_get_angle_sec(COM2, 0);
cprintf("Res = %d\n\n",res);
 
ch = keyb_getch(BLOCK);
 
/*----------------------------------------- */
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 0,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 0);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK); */
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 1,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 0);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK); */
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 2,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 2);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK); */
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 3,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 3);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK); */
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 4,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 4);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK); */
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 5,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 5);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK); */
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 6,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 6);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK); */
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 7,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 7);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK);*/
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 8,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 8);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK);*/
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 9,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 9);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK);*/
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 10,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 10);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK);*/
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 11,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 11);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK);*/
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 12,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 12);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK);*/
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 13,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 13);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK);*/
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 14,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 14);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK);*/
 
cprintf("servo_set_angle_sec\n");
res = servo_set_angle_sec(COM2, 15,ANGLE2SEC(0,0,0));
cprintf("Res = %d\n",res);
/*cprintf("servo_store_default_position\n");
res = servo_store_default_position(COM2, 15);
cprintf("Res = %d\n",res);
 
ch = keyb_getch(BLOCK);*/
 
servo_close(COM2);
 
return 0;
 
}
 
/demos/trunk/servo/makefile
7,10 → 7,15
endif
include $(BASE)/config/config.mk
 
PROGS = servo
PROGS = ctrl time load
 
include $(BASE)/config/example.mk
 
servo:
make -f $(SUBMAKE) APP=servo INIT= OTHEROBJS="initfile.o leg.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __SERVO__"
ctrl:
make -f $(SUBMAKE) APP=ctrl INIT= OTHEROBJS="initfile.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __SERVO__"
 
time:
make -f $(SUBMAKE) APP=time INIT= OTHEROBJS="initfile.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __SERVO__"
 
load:
make -f $(SUBMAKE) APP=load INIT= OTHEROBJS="initfile.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __SERVO__"
/demos/trunk/servo/README
0,0 → 1,2
TODO