void select_speed()
{
int select_frag=0;
double selecter=500.0;
while(select_frag==0){
calc_senc();
if(PORTE.PORT.BIT.B0 == 0){
TIMER_WAIT(200);
LED2=~LED2;
selecter+=100.0;
}
if(cari_SEN[1] > 20.0){
Desire_vel_nomal=selecter;
first_speed=Desire_vel_nomal;
if(Desire_vel < 800.0){
max_ERR=90.0;
min_ERR=-90.0;
}
else if(Desire_vel >= 800.0 ){
max_ERR=130.0;
min_ERR=-130.0;
}
select_frag=1;
LED_manager(7);
}
}
TIMER_WAIT(1000);
reset();
}
void countdown()
{
reset();
LED1=ON; LED2=ON;
TIMER_WAIT(300);
LED1=OFF; LED2=OFF;
TIMER_WAIT(300);
LED1=ON; LED2=ON;
TIMER_WAIT(300);
LED3=ON;
TIMER_WAIT(1000);
reset();
}
void out_gyro()
{
short roop_frag=0;
short deg=0;
short Gy=0;
short gyr=0;
short ave_gyr=0;
short i=0;
reset();
while(roop_frag==0){
if(PORTE.PORT.BIT.B0 == 0){
TIMER_WAIT(2000);
for(i=0; i<100; i++)
zero_gyro=get_gyro();
start0=1;
roop_frag=1;
}
}
while(1){
Gy=(short)GYRO_ERR;
gyr=(short)gyro2;
ave_gyr=(short)ave_gyro;
dec_out(Gy, 5); outs(" ");
dec_out(deg, 5); outs(" ");
dec_out(gyr, 5); outs(" ");
dec_out(ave_gyr, 5); outs(" ");
outs("\n");
}
}
void out_senc_drive_mot()
{
short i;
TIMER_WAIT(2000);
search_max();
start0=1;
mot_STB(START_A);
mot_frag=1;
TIMER_WAIT(2000);
while(1){
MTU0.TGRA=100;
MTU0.TGRC=100;
for(i=1; i<11; i++){
dec_out((short)ave_SEN[i], 4); outs(" ");
}
outs("\n");
}
}
void main()
{
int mode_count=1;
set();
init_goal_frag();
init_thre();
PORT9.DR.BIT.B3=0;
mot_STB(STOP);
Battery_Check();
ENC_R=3300;
while(1)
{
if(ENC_R > 1600*2 && ENC_R < 1600*3){
LED_manager(1);
mode_count=1;
}
if(ENC_R > 1600*3 && ENC_R < 1600*4){
LED_manager(2);
mode_count=2;
}
if(ENC_R > 1600*4 && ENC_R < 1600*5){
LED_manager(3);
mode_count=3;
}
if(ENC_R > 1600*5 && ENC_R < 1600*6){
LED_manager(4);
mode_count=4;
}
if(ENC_R > 1600*6 && ENC_R < 1600*7){
LED_manager(5);
mode_count=5;
}
if(ENC_R > 1600*7 && ENC_R < 1600*8){
LED_manager(6);
mode_count=6;
}
if(ENC_R > 1600*8 && ENC_R < 1600*9){
LED_manager(7);
mode_count=7;
}
if(ENC_R > 1600*9)
ENC_R=(1600*2)+1;
if(PORTE.PORT.BIT.B0 == 0){
TIMER_WAIT(500);
ch_mode(mode_count);
reset();
}
}
}
void stop_manager() //停止処理
{
while(stop_count!=200){
mot_drive(ERR, sub_ERR, PRE_ERR, SUM_ERR, GYRO_ERR, SPEED_ERR_R, SPEED_SUM_ERR_R, SPEED_ERR_L, SPEED_SUM_ERR_L, 0.0, 0.0);
}
init_goal_frag();
mot_brake();
TIMER_WAIT(1000);
mot_STB(STOP);
}
void line_check()
{
short i;
double max=0.0;
TIMER_WAIT(2000);
while(1){
get_senc();
dec_out((short)SEN[goal], 4); outs("\n");
}
}
void select_goal()
{
int select_frag=0;
double selecter=0.20;
while(select_frag==0){
calc_senc();
if(PORTE.PORT.BIT.B0 == 0){
TIMER_WAIT(200);
LED2=~LED2;
selecter+=0.01;
}
if(cari_SEN[1] > 20.0){
K_goal=selecter;
select_frag=1;
LED_manager(7);
}
}
TIMER_WAIT(1000);
reset();
}
void select_P()
{
int select_frag=0;
double selecter=5.0;
while(select_frag==0){
calc_senc();
if(PORTE.PORT.BIT.B0 == 0){
TIMER_WAIT(200);
LED2=~LED2;
selecter+=1.0;
}
if(cari_SEN[1] > 20.0){
K_P=selecter;
first_K_P=K_P;
select_frag=1;
LED_manager(7);
}
}
TIMER_WAIT(1000);
reset();
}
void out_curve() //ログチェック
{
short i=0;
TIMER_WAIT(2000);
dec_out(ST_count, 8); outs("\n");
for(i=0; i<201; i++){
// dec_out(ST_check[i], 8); outs(" ");
// dec_out((short)sub_SEN_check[i], 8); outs(" ");
// dec_out(vel_check[i], 8); outs(" ");
dec_out(ENC_check[i], 8); outs(" ");
dec_out((short)ENC_dis[i], 8); outs("\n");
}
}
void out_ENC()
{
TIMER_WAIT(1000);
ENC_R=0; //max:65535
ENC_L=0;
start0=1;
while(1){
dec_out(now_ENC_R, 8); outs(" ");
dec_out(now_ENC_L, 8); outs(" ");
dec_out((now_ENC_R-now_ENC_L)/6400, 8); outs(" ");
outs("\n");
}
}
short select_mode()
{
short para_frag=0;
short para_count=1;
ENC_R=3201;
while(para_frag==0){
if(ENC_R > 1600*2 && ENC_R < 1600*3){
LED_manager(1);
para_count=1;
}
if(ENC_R > 1600*3 && ENC_R < 1600*4){
LED_manager(2);
para_count=2;
}
if(ENC_R > 1600*4 && ENC_R < 1600*5){
LED_manager(3);
para_count=3;
}
if(ENC_R > 1600*5 && ENC_R < 1600*6){
LED_manager(4);
para_count=4;
}
if(ENC_R > 1600*6 && ENC_R < 1600*7){
LED_manager(5);
para_count=5;
}
if(ENC_R > 1600*7 && ENC_R < 1600*8){
LED_manager(6);
para_count=6;
}
if(ENC_R > 1600*8)
ENC_R=(1600*2)+1;
if(PORTE.PORT.BIT.B0 == 0){
TIMER_WAIT(500);
reset();
para_frag=1;
}
}
return para_count;
}
void Battery_Check()
{
short battery;
battery=get_Batt();
if(battery >= 757){
LED_manager(7);
LED1=ON; LED2=ON; LED3=ON;
}
if(battery < 757 && battery >= 720){
LED_manager(3);
}
if(battery < 720){ //7.4V未満になったら未満強制停止
LED_manager(4);
while(1){
mot_STB(STOP);
}
}
TIMER_WAIT(1500);
reset();
}
void out_senc()
{
short i;
TIMER_WAIT(2000);
LED_test=1;
while(1){
/*
calc_senc();
if(ave_SEN[corner] > 70.0)
LED_manager(1);
if(ave_SEN[corner] < 70.0 && ave_SEN[corner] > 60.0)
LED_manager(2);
if(ave_SEN[corner] < 60.0 && ave_SEN[corner] > 50.0)
LED_manager(3);
if(ave_SEN[corner] < 50.0)
LED_manager(4);
for(i=1; i<11; i++){
dec_out((short)ave_SEN[i], 4); outs(" ");
}
outs("\n");
*/
}
}
void search_max()
{
int i;
short loop_frag=0;
while(loop_frag==0){
if(PORTE.PORT.BIT.B0 == 0)
loop_frag=1;
else{}
}
TIMER_WAIT(1000);
LED1=OFF;
ENC_R=0;
ENC_L=0;
mot_STB(START_A);
LED3=ON;
for(i=1; i<9; i++){
SEN_max[i]=1;
SEN_min[i]=10;
}
cari_frag=1;
while(ENC_R < 10000){
MOT_ctr_R1=1;
MOT_ctr_R2=0;
MOT_ctr_L1=0;
MOT_ctr_L2=1;
MTU0.TGRC=100 - (0.2*(ENC_R-ENC_L));
MTU0.TGRA=100 + (0.2*(ENC_R-ENC_L));
for(i=1; i<9; i++){
if(SEN_max[i] < SEN[i])
SEN_max[i]=SEN[i];
if(SEN[i] > 0 && SEN_min[i] > SEN[i])
SEN_min[i]=SEN[i];
}
}
cari_frag=0;
mot_brake();
ENC_R=40000;
ENC_L=40000;
TIMER_WAIT(200);
MOT_ctr_R1=0;
MOT_ctr_R2=1;
MOT_ctr_L1=1;
MOT_ctr_L2=0;
cari_frag=1;
while((40000-ENC_R) < 10000){
MTU0.TGRC=100 - (0.0001*(ENC_R-ENC_L));
MTU0.TGRA=100 + (0.0001*(ENC_R-ENC_L));
for(i=1; i<9; i++){
if(SEN_max[i] < SEN[i])
SEN_max[i]=SEN[i];
if(SEN[i] > 0 && SEN_min[i] > SEN[i])
SEN_min[i]=SEN[i];
}
}
cari_frag=0;
mot_brake();
ENC_R=0;
ENC_L=0;
MTU0.TGRC=0;
MTU0.TGRA=0;
TIMER_WAIT(1000);
LED3=OFF;
mot_STB(STOP);
reset();
for(i=1; i<9; i++)
SEN_minmax[i]=SEN_max[i]-SEN_min[i];
}
int main(int argc, char *argv[])
{
int option;
int exitsig;
db_clt_typ *pclt;
char hostname[MAXHOSTNAMELEN+1];
int interval = 50; /// Number of milliseconds between saves
posix_timer_typ *ptimer; /* Timing proxy */
char *domain = DEFAULT_SERVICE; // usually no need to change this
int xport = COMM_OS_XPORT; // set correct for OS in sys_os.h
int verbose = 0;
char zero_array[200];
int i;
memset(zero_array, 0, sizeof(zero_array));
/* Read and interpret any user switches. */
while ((option = getopt(argc, argv, "i:v")) != EOF) {
switch(option) {
case 'i':
interval = atoi(optarg);
break;
case 'v':
verbose = 1;
break;
default:
printf("Usage: %s\n", argv[0]);
printf(" -d <trip directory> \n");
printf(" -m <file time in minutes> \n");
printf(" -l <no. lines to save to file> \n");
printf(" -t <loop time in ms> \n");
exit(EXIT_FAILURE);
}
}
/* Log in to the database (shared global memory). Default to the
* the current host. Assumes other processes that create variables
* are already running, since list is NULL.
*/
get_local_name(hostname, MAXHOSTNAMELEN);
if ((pclt = db_list_init(argv[0], hostname, domain, xport,
db_controller_list, NUM_CONTROLLER_VARS, NULL, 0)) == NULL) {
printf("Database initialization error in %s.\n", argv[0]);
exit(EXIT_FAILURE);
}
for (i = 0; i < NUM_CONTROLLER_VARS; i++){
db_clt_write(pclt,
db_controller_list[i].id,
db_controller_list[i].size,
zero_array);
}
/* Setup a timer for every 'interval' msec. */
if ((ptimer = timer_init(interval, DB_CHANNEL(pclt) )) == NULL) {
printf("Unable to initialize wrfiles timer\n");
exit(EXIT_FAILURE);
}
if(( exitsig = setjmp(exit_env)) != 0) {
db_list_done(pclt, db_controller_list, NUM_CONTROLLER_VARS, NULL, 0);
exit(EXIT_SUCCESS);
} else
sig_ign(sig_list, sig_hand);
for(;;) {
TIMER_WAIT(ptimer);
}
}
void trace() //一週目
{
short start_frag=0;
short trace_roop_frag=0;
short i=0;
short permit_goal=0;
circl_count=1;
LED1=ON;
TIMER_WAIT(1000);
LED1=OFF;
ch_para();
//ch_para2(0);
select_P();
// select_sub();
select_I();
// select_D();
// select_G();
select_goal();
// select_corner();
select_speed();
while(trace_roop_frag==0){
init_thre();
calc_senc();
if(cari_SEN[1] > 20.0){
if(Dispermit_goal==1){
start_frag=1;
LED2=ON;
TIMER_WAIT(500);
}
else{
LED2=ON;
permit_goal=1;
}
}
if(cari_SEN[6] > 20.0 && permit_goal==1){
start_frag=1;
LED3=ON;
TIMER_WAIT(500);
}
else if(cari_SEN[6] > 20.0 && permit_goal==0){ //ゴールさせない
LED3=ON;
Dispermit_goal=1;
}
if(start_frag==1){
countdown();
TIMER_WAIT(1000);
for(i=0; i<201; i++){
ENC_dis[i]=0.0;
ENC_check[i]=0;
RL_check[i]=3;
}
for(i=0; i<5; i++)
SC_box[i]=ST;
zero_gyro=get_gyro();
ENC_R=15000;
ENC_sub_R=0;
ENC_L=15000;
ENC_sub_L=0;
EncTimeCount=0;
ENC_count=0;
dis_count=0;
mot_STB(START_A);
start0=1;
mot_frag=1;
LED1=OFF;
while(stop_frag==0){
if(PORTE.PORT.BIT.B0 == 0){
stop_frag=1;
TIMER_WAIT(500);
}
}
stop_manager();
trace_roop_frag=1;
}
else{}
}
}
void trace2() //二週目
{
short start_frag=0;
short trace_roop_frag=0;
short permit_goal=0;
circl_count=2;
LED2=ON;
TIMER_WAIT(1000);
LED2=OFF;
// ch_para2(0);
ch_para();
select_P();
select_I();
// select_D();
// select_G();
select_goal();
// select_corner();
select_speed();
while(trace_roop_frag==0){
init_thre();
calc_senc();
if(cari_SEN[1] > 20.0){
if(Dispermit_goal==1){
start_frag=1;
LED2=ON;
TIMER_WAIT(500);
}
else{
LED2=ON;
permit_goal=1;
}
}
if(cari_SEN[6] > 20.0 && permit_goal==1){
start_frag=1;
LED3=ON;
TIMER_WAIT(500);
}
else if(cari_SEN[6] > 20.0 && permit_goal==0){ //ゴールさせない
LED3=ON;
Dispermit_goal=1;
}
if(start_frag==1){
countdown();
TIMER_WAIT(1000);
zero_gyro=get_gyro();
ENC_R=15000;
ENC_sub_R=0;
ENC_L=15000;
ENC_sub_L=0;
EncTimeCount=0;
ENC_count=0;
dis_count=0;
mot_STB(START_A);
start0=1;
mot_frag=1;
while(stop_frag==0){}
stop_manager();
trace_roop_frag=1;
}
else{}
}
}
void trace3() //3週目
{
int start_frag[3];
int trace_roop_frag=0;
int roop_frag=0;
short para_frag=0;
circl_count=3;
LED3=ON;
TIMER_WAIT(1000);
LED3=OFF;
while(trace_roop_frag==0){
if(PORTE.PORT.BIT.B0 == 0){
LED1=OFF;
TIMER_WAIT(500);
while(roop_frag==0){
if(para_frag==0){
select_P();
ch_para();
para_frag=1;
}
calc_senc();
if(cari_SEN[1] > 40){
start_frag[1]=1;
LED2=ON;
}
if(start_frag[1]==1){
if(cari_SEN[6] > 40){
start_frag[2]=1;
LED3=ON;
TIMER_WAIT(500);
}
}
if(start_frag[2]==1){
countdown();
TIMER_WAIT(1000);
mot_STB(START_A);
zero_gyro=get_gyro();
ENC_R=0;
ENC_sub_R=0;
ENC_L=0;
ENC_sub_L=0;
start0=1;
while(stop_frag==0){}
stop_manager();
trace_roop_frag=1;
roop_frag=1;
}
else{}
}
}
else{}
}
}
int main( int argc, char *argv[] )
{
int ch;
int pgn; /* Parameter Group Number */
struct j1939_pdu pdu; /* Protocol Data Unit */
posix_timer_typ *ptimer; /* Timing proxy */
int interval = 20;
int slot_number = 1;
int i;
int do_send = 0; /* receives by default */
int msg_count = 0;
int fd; /* "file descriptor" for CAN device */
char *filename = "/dev/can1";
int extended = -1;
int flags = O_RDONLY;
while ((ch = getopt(argc, argv, "d:f:i:n:p:st:")) != EOF) {
switch (ch) {
case 'f': filename = strdup(optarg);
break;
case 'i': interval = atoi(optarg);
break;
case 'n': slot_number = atoi(optarg);
break;
case 'p': pgn = atoi(optarg);
break;
case 's': do_send = 1;
flags = O_WRONLY;
break;
case 't': interval = atoi(optarg);
break;
default: printf(
"Usage: %s [-p PGN number -i interval]\n",
argv[0]);
exit(1);
break;
}
}
fprintf(stderr, "request PGN %d (0x%x)\n", pgn, pgn);
/* Initialize CAN device */
fd = init_can(filename, flags, NULL);
if (fd < 0) {
printf("Error opening device %s for input\n", filename);
exit(EXIT_FAILURE);
}
if (do_send) {
int chid = ChannelCreate(0);
if ((ptimer = timer_init(interval, chid )) == NULL) {
printf("Unable to initialize canj1939 timer\n");
exit(EXIT_FAILURE);
}
}
if( setjmp( exit_env ) != 0 ) {
printf("canj1939 exits, %s %d\n",
do_send?"sent":"received", msg_count);
close_can(&fd);
exit( EXIT_SUCCESS );
} else
sig_ign( sig_list, sig_hand );
if (do_send) {
/* assemble request from arguments */
pdu.priority = 6;
pdu.R = 0;
pdu.DP = 0;
pdu.pdu_format = HIBYTE(RQST);
pdu.pdu_specific = GLOBAL;
pdu.src_address = 249; /* Service Tool address */
pdu.numbytes = 3;
/* current PGNs are only two bytes, but field is 3 bytes,
LSB first */
pdu.data_field[0] = LOBYTE(pgn);
pdu.data_field[1] = HIBYTE(pgn);
pdu.data_field[2] = 0;
for (i = 3; i < 8; i++)
pdu.data_field[i] = 0xff;
}
/* state code and slot number not used */
if (do_send) {
for ( ; ; ) {
send_can(fd, &pdu, slot_number);
TIMER_WAIT( ptimer );
msg_count++;
}
}
else {
for ( ; ; ) {
if (!receive_can(fd, &pdu, &extended, &slot_number))
printf("call to receive_can returned error\n");
else {
printf("Pr %d PF %d PS %d", pdu.priority,
pdu.pdu_format, pdu.pdu_specific);
printf("SA %d: ", pdu.src_address);
for (i = 0; i < 8; i++)
printf("%d ", pdu.data_field[i]);
printf(" extended %d\n", extended);
msg_count++;
}
}
}
}
void ch_para()
{
short para_frag=0;
short para_count=0;
ENC_R=1600;
while(para_frag==0){
if(ENC_R > 1600*2 && ENC_R < 1600*3){
LED_manager(1);
para_count=0;
}
if(ENC_R > 1600*3 && ENC_R < 1600*4){
LED_manager(2);
para_count=1;
}
if(ENC_R > 1600*4 && ENC_R < 1600*5){
LED_manager(3);
para_count=2;
}
if(ENC_R > 1600*5 && ENC_R < 1600*6){
LED_manager(4);
para_count=3;
}
if(ENC_R > 1600*6)
ENC_R=(1600*2)+1;
if(PORTE.PORT.BIT.B0 == 0){
TIMER_WAIT(500);
reset();
para_frag=1;
}
}
switch(para_count){
case 0: speed=200.0; //220
K_P=8.5; //13.5
first_K_P=K_P;
K_P_sub=0.0;
K_P_add1=9.0;
K_P_add2=12.0;
K_D=0.1; //0.0002
K_I=0.000003; //12
K_G=0.8;
first_K_G=K_G;
K_E=0.00001;
K_goal=6.5;
K_corner=2.8;
first_corner=K_corner;
first_speed=800.0;
Accel=100.0;
Accel_in=Accel+3000;
Desire_vel_nomal=800.0;
Target_vel=1200.0;
speed_para[0]=1200.0;
speed_para[1]=1800.0;
speed_para[2]=2000.0;
break;
case 1: speed=230.0; //220
K_P=8.5; //13.5
first_K_P=K_P;
K_P_sub=1.00;
K_P_add1=9.0;
K_P_add2=12.0;
K_D=0.0; //0.0002
K_I=0.000003; //12
K_G=0.9;
first_K_G=K_G;
K_E=0.00001;
K_goal=6.5;
K_corner=3.0;
first_speed=900.0;
Accel=1000.0;
Accel_in=Accel+3000;
Target_vel=2000.0;
speed_para[0]=1500.0;
speed_para[1]=2000.0;
speed_para[2]=2500.0;
break;
case 2: speed=270.0; //220
K_P=8.5; //13.5
first_K_P=K_P;
K_P_sub=1.00;
K_P_add1=9.0;
K_P_add2=12.0;
K_D=0.0; //0.0002
K_I=0.000003; //12
K_G=0.9;
first_K_G=K_G;
K_E=0.00001;
K_goal=6.5;
K_corner=3.0;
first_speed=900.0;
Accel=6000.0;
Accel_in=Accel;
Target_vel=2000.0;
speed_para[0]=1000.0;
speed_para[1]=2000.0;
speed_para[2]=4000.0;
break;
case 3: speed=150.0; //220
K_P=8.5; //13.5
first_K_P=K_P;
K_P_sub=1.00;
K_P_add1=9.0;
K_P_add2=12.0;
K_D=3.0; //0.0002
K_I=0.000003; //12
K_G=0.9;
first_K_G=K_G;
K_E=0.00001;
K_goal=6.5;
K_corner=2.8;
first_speed=800.0;
Accel=1500.0;
Accel_in=Accel;
Target_vel=1200.0;
speed_para[0]=1000.0;
speed_para[1]=1200.0;
speed_para[2]=1800.0;
break;
}
reset();
}