#define __KERNEL__ #define __RT__ #define MODULE #include #include #include #include #include #include #include MODULE_LICENSE("GPL"); EXPORT_NO_SYMBOLS; #define MAXPICTASK 5000 #define OLDBALANCE 0 #define TMEM 1023//511 //256 //256 //memory for threads //#define M100 2000000 //pulse every 4ms //#define M75 1500000 //pulse 3ms of 4ms //#define M50 1000000 //pulse 2ms of 4ms //#define M25 500000 //pulse 1ms of 4ms //#define M100 3000000 //pulse every 4ms //#define M75 22500000 //pulse 3ms of 4ms //#define M50 1500000 //pulse 2ms of 4ms //#define M25 750000 //pulse 1ms of 4ms //#define M100 4000000 //pulse every 4ms //#define M75 3000000 //pulse 3ms of 4ms //#define M50 2000000 //pulse 2ms of 4ms //#define M25 1000000 //pulse 1ms of 4ms //#define M100 5000000 //pulse every 4ms //#define M75 3750000 //pulse 3ms of 4ms //#define M50 2500000 //pulse 2ms of 4ms //#define M25 1250000 //pulse 1ms of 4ms //#define M100 6000000 //pulse every 4ms //#define M75 4500000 //pulse 3ms of 4ms //#define M50 3000000 //pulse 2ms of 4ms //#define M25 1500000 //pulse 1ms of 4ms #define M100 10000000 //pulse every 4ms #define M75 75000000 //pulse 3ms of 4ms #define M50 50000000 //pulse 2ms of 4ms #define M25 25000000 //pulse 1ms of 4ms #define M1 2000000 //pulse 1ms of 4ms static RT_TASK tasks[20]; static RT_TASK pictask[MAXPICTASK]; unsigned char balsend0a[256]; unsigned char balsend0d[256]; unsigned char balget0a[256]; unsigned char balget0d[256]; int balance; int pictasknum,getimages; unsigned char picport[256]; //0-f x 2 0,1...16,0,16,32,48,...240 int curmotor; int bav[5],tav[5]; int akal0a,akal0d; int tavm0a[10],tavmk0a; //last 10 balance readings int tavm0d[10],tavmk0d; //last 10 balance readings int calibrate; RTIME nextpulse0a,nextpulse0d; static void picthread_code(int inst) { unsigned char newinst[2]; unsigned char data,addr,msk; //rt_printk( "\ninst= %x",inst); // 8 x 8 x 8 mask x addr x data //msk=(inst&0x3f0000)>>0x10; msk=(inst&0xff0000)>>0x10; addr=(inst&0xff00)>>0x8; data=(inst&0xff); //msk=11111100 addr=00000000 data=00000010 //newinst[0]=(addr>>2); newinst[0]=addr; //keep what is on other 4 port bits //newinst[1]=(addr<<6)|(picport[addr]&msk); //newinst[1]|=data; newinst[1]=picport[addr]&msk; newinst[1]|=data; //picport[addr]=newinst[1]&0x3f; picport[addr]=newinst[1]; rt_com_write(0, &newinst[0], 2); //there is a problem sending 2 bytes together. //rt_com_write(0, &newinst[0], 1); rt_printk( "inst sent = %x %x \n",newinst[0],newinst[1]); //rt_printk( "\npicport %x = %x",addr,picport[addr]); //rt_printk( "\npicport %x = %x, msk= %x",addr,picport[addr],msk); //rt_com_write(0, &newinst[1], 1); rt_task_delete(rt_whoami()); rt_task_wait_period (); //for some reason need } //end picthread_code static void turn_motor_speed(int motornum,int dir,RTIME dur,RTIME cycle, RTIME duty) { //dur=1000000000 1 s, cycle=100000 100us ,duty=50000 50us unsigned int inst,inst2; unsigned int addr,data,stopdata; int mb; RTIME a; int knt; //make a new task with the motor instruction //addr=motornum/3; //pic port number addr=motornum/4; //pic port number //mb=(motornum%3)*2; //0,2,4 how much to shift mb=(motornum%4)*2; //0,2,4,8 how much to shift stopdata=0; //2 1's will not be possible if (dir>0) { data=(0x1<=MAXPICTASK) { pictasknum=0; } rt_task_init(&pictask[pictasknum],picthread_code,inst2,TMEM,0,0,0); rt_task_make_periodic_relative_ns(&pictask[pictasknum],a+duty,0); pictasknum++; if (pictasknum>=MAXPICTASK) { pictasknum=0; } knt++; } //end for //rt_printk( "\nmotor speed inst=%x inst2=%x num=%d\n",inst,inst2,knt); } //end turn_motor_speed static void turn_motor_speed_t(int motornum,int dir,RTIME st,RTIME dur,RTIME cycle, RTIME duty) { //dur=1000000000 1 s, cycle=100000 100us ,duty=50000 50us unsigned int inst,inst2; unsigned int addr,data,stopdata; int mb; RTIME a; int knt; //make a new task with the motor instruction addr=motornum/4; //pic port number mb=(motornum%4)*2; //0,2,4,8 how much to shift stopdata=0; //2 1's will not be possible if (dir>0) { data=(0x1<=MAXPICTASK) { pictasknum=0; } rt_task_init(&pictask[pictasknum],picthread_code,inst2,TMEM,0,0,0); rt_task_make_periodic_relative_ns(&pictask[pictasknum],a+duty,0); pictasknum++; if (pictasknum>=MAXPICTASK) { pictasknum=0; } knt++; } //end for //rt_printk( "\nmotor speed inst=%x inst2=%x num=%d\n",inst,inst2,knt); } //end turn_motor_speed_t static void turn_motor_speed2(int motornum,int dir,RTIME dur,long cycle, long duty2) { //dur=1000000000 1 s, cycle=100000 100us ,duty=50000 50us unsigned int inst,inst2; unsigned int addr,data,stopdata; int mb; RTIME a; RTIME duty; int knt; duty=cycle*duty2/100; //printk(KERN_INFO,"\nduty= %d cycle= %d duty2= %d\n",duty,cycle,duty2); //make a new task with the motor instruction addr=motornum/4; //pic port number mb=(motornum%4)*2; //0,2,4,8 how much to shift stopdata=0; //2 1's will not be possible if (dir>0) { data=(0x1<=MAXPICTASK) { pictasknum=0; } rt_task_init(&pictask[pictasknum],picthread_code,inst2,TMEM,0,0,0); rt_task_make_periodic_relative_ns(&pictask[pictasknum],a+duty,0); pictasknum++; if (pictasknum>=MAXPICTASK) { pictasknum=0; } knt++; } //end for //rt_printk( "\nmotor speed inst=%x inst2=%x num=%d\n",inst,inst2,knt); } //end turn_motor_speed static void set_motor(int motornum,int dir) { unsigned int inst; unsigned int addr,data,stopdata; int mb; //make a new task with the motor instruction addr=motornum/4; //pic port number mb=(motornum%4)*2; //0,2,4,8 how much to shift //2 1's will not be possible if (dir>0) { data=(0x1<=MAXPICTASK) { pictasknum=0; } } //end if dir!=0 else { //send stop inst=(~(0x3<=MAXPICTASK) { pictasknum=0; } //rt_printk( "\nset motor inst=%x",inst); } } //end set_motor static void turn_motor(int motornum,int dir,RTIME dur) { unsigned int inst; unsigned int addr,data,stopdata; int mb; //make a new task with the motor instruction addr=motornum/4; //pic port number mb=(motornum%4)*2; //0,2,4,8 how much to shift stopdata=0; //zero two motor bits //rt_printk( "\nmotor %d addr=%x\n",motornum,addr); //rt_printk( "\nmotor %d dur=%d\n",motornum,dur); //2 1's will not be possible if (dir>0) { data=(0x1<=MAXPICTASK) { pictasknum=0; } } //end if dir!=0 inst=((~(0x3<=MAXPICTASK) { pictasknum=0; } } //end turn_motor static void turn_motor_t(int motornum,int dir,RTIME st,RTIME dur) { unsigned int inst; unsigned int addr,data,stopdata; int mb; //make a new task with the motor instruction addr=motornum/4; //pic port number mb=(motornum%4)*2; //0,2,4,8 how much to shift stopdata=0; //zero two motor bits //rt_printk( "\nmotor %d addr=%x\n",motornum,addr); //rt_printk( "\nmotor %d dur=%d\n",motornum,dur); //2 1's will not be possible if (dir>0) { data=(0x1<=MAXPICTASK) { pictasknum=0; } } //end if dir!=0 inst=((~(0x3<=MAXPICTASK) { pictasknum=0; } } //end turn_motor_t #if 0 static int my_handler(unsigned int fifo, int rw) { int err,i; char Key; unsigned char inst[200]; char gbyt; while ((err = rtf_get(0, &Key, 1)) == 1) { //rtf_put (msg.task + COMMAND_FIFO + 1, &msg, sizeof(msg)); //rt_printk("FIFO handler: got key %x\n", Key); //rt_task_resume(&tasks[msg.task]); switch (Key) { case 0x30: //0 //stop all motors //turn_motor(0,0,0); // set_motor(0,0); // printk(KERN_INFO "stop motor 0 instruction sent\n"); curmotor=0; inst[0]=0x00; //addr=0 inst[1]=0x01; //use data bit rt_com_write(0, &inst[0], 2); break; case 0x31: //1 //set motor 0 to 1 //set_motor(0,1); //printk(KERN_INFO "set motor key got\n"); curmotor=1; break; case 0x32: //2 turn motor 0 for 2 seconds //set_motor(0,-1); //printk(KERN_INFO "set key got\n"); curmotor=2; break; case 0x33: //3 turn motor 0 for 2 seconds //inst[0]=0x00; //inst[1]=0x01; // //rt_com_write(0, &inst[0], 2); curmotor=3; break; case 0x34: //4 turn motor 0 for 2 seconds /* inst[0]=0x00; inst[1]=0x02; // rt_com_write(0, &inst[0], 2);*/ curmotor=4; break; case 0x35: //5 turn motor 0 for 2 useconds curmotor=5; break; case 0x36: //6 turn motor 0 for 2 useconds curmotor=6; curmotor=1019; break; case 0x37: //7 turn motor 0 for 2 useconds curmotor=7; curmotor=1020; break; case 0x38: //8 turn motor 0 for 2 useconds curmotor=8; curmotor=56; break; case 0x39: //9 turn motor 0 for 2 useconds //curmotor=9; curmotor=60; //curmotor=56; break; case 0x51: //left arrow //10ms full pulse //10ms half pulse (5ms full pulse, or 2 2.5ms full pulses, etc...) //what is maximum pulse for half speed? turn_motor(curmotor,1,10000000); //pulse motor 0 dir 1 for 10ms //turn_motor_speed2(int motornum,int dir,RTIME dur,RTIME cycle, int duty2) //turn_motor_speed_t(curmotor,1,0,40000000,1000000, 500000); //could be 10ms 100us cycle (50us/100us) high pitch //higher cycle = better (10ms (1ms/.5ms)) not as high //1/2 speed (1ms/.5ms), 1/4 speed (1/.25ms), 1/8 speed (1ms/.125) //turn_motor_speed_t(curmotor,1,1000000000,1000000000,1000000, 250000); //motor=curmotor, dir=1, dur=10ms, cycle=10us, duty=50% // printk(KERN_INFO "turn motor instruction sent\n"); break; case 0x53: //right arrow turn_motor(curmotor,-1,10000000); //pulse motor 0 dir 1 for 10ms //turn_motor(curmotor,-1,400000000); //pulse motor 0 dir 1 for 1ms //turn_motor(curmotor,-1,400000000); //never more than .4s!!! //turn_motor_speed_t(curmotor,-1,0,40000000,1000000, 100000); //turn_motor_speed_t(curmotor,-1,0,500000000,10000000, 1000000); // printk(KERN_INFO "turn motor instruction sent\n"); break; case 0x61: //a turn left 90 degrees turn_motor(0,-1,100000000); //pulse motor 0 dir 1 for 100ms break; case 0x62: //b = balance balance=!balance; if (balance) { //send every .1 sec /*rt_task_make_periodic_relative_ns(&tasks[0],0,100000000); //get every .12 sec rt_task_make_periodic_relative_ns(&tasks[1],20000000,100000000); */ //2 fast // rt_task_make_periodic_relative_ns(&tasks[0],0,100000000); // rt_task_make_periodic_relative_ns(&tasks[1],20000000,100000000); //every .05s 50ms rt_task_make_periodic_relative_ns(&tasks[0],0,50000000); rt_task_make_periodic_relative_ns(&tasks[1],25000000,50000000); tavmk0d=-1; rt_task_make_periodic_relative_ns(&tasks[2],25000000,50000000); rt_task_make_periodic_relative_ns(&tasks[3],50000000,50000000); tavmk0a=-1; //30000000 - sending motor command for ~ 60ms printk(KERN_INFO "balance on\n"); } else { rt_task_suspend(&tasks[0]); rt_task_suspend(&tasks[1]); rt_task_suspend(&tasks[2]); rt_task_suspend(&tasks[3]); printk(KERN_INFO "balance off\n"); } break; case 0x64: //d turn right 90 degrees turn_motor(0,1,100000000); //pulse motor 0 dir 1 for 100ms break; case 0x65: //e turn left 90 degrees tenth speed // turn_motor_speed(0,1,800000000,10000000,500000); //slow turn_motor_speed(0,1,1800000000,10000000,1000000); //1/4 break; //pulse motor 0 dir 1 for 100ms cycle=100us duty=50us case 0x66: //f turn left 90 degrees half speed // turn_motor_speed(0,1,480000000,10000000,2500000); //1/4 //get input from feet inst[0]=0x02; inst[1]=0x00; inst[0]=0x10; inst[1]=0x00; inst[0]=0x00; inst[1]=0x01; rt_com_write(0, &inst[0], 2); err=rt_com_read(0, &gbyt, 1); rt_printk("input= %d %x\n",err,gbyt&0xf); break; //pulse motor 0 dir 1 for 100ms cycle=100us duty=50us case 0x68: //h turn right 90 degrees half speed //turn_motor_speed(0,-1,80000000,100000,50000); //50us is too fast //turn_motor_speed(0,-1,230000000,1000000,500000); //500us is possibly also too fast // turn_motor_speed(0,-1,300000000,10000000,5000000); turn_motor_speed(0,-1,520000000,10000000,2500000); //1/4 //turn_motor_speed(0,-1,300000000,20000000,10000000); //turn_motor_speed(0,-1,300000000,20000000,5000000); //1/4 // turn_motor_speed(0,-1,300000000,40000000,20000000); //little choppy break; //pulse motor 0 dir 1 for 100ms cycle=100us duty=50us case 0x69: //i = get image (was input) #if 0 inst[0]=0x06; inst[1]=0xa2; // rt_com_write(0, &inst[0], 2); printk(KERN_INFO "get input sent\n"); #endif if (getimages) { rt_task_suspend(&tasks[2]); printk(KERN_INFO "stop get images....\n"); getimages=!getimages; } else { //get 10 images/second 30 images/second rt_task_make_periodic_relative_ns(&tasks[2],0,100000000); printk(KERN_INFO "get images....\n"); getimages=!getimages; } break; case 0x6a: //j inst[0]=0x06; //inst[1]=0x81; // rt_com_write(0, &inst[0], 1); printk(KERN_INFO "get input sent\n"); break; case 0x6b: //k calibrate=!calibrate; //inst[0]=0x80;//0x06; //inst[1]=0x81; // //rt_com_write(0, &inst[0], 1); printk(KERN_INFO "calibrate=%d\n",calibrate); break; case 0x6c: //l inst[0]=0x06; inst[1]=0xa2; inst[2]=0x06; inst[3]=0xa2; rt_com_write(0, &inst[0], 4); printk(KERN_INFO "get input sentx2\n"); break; case 0x6d: //m for(i=0;i<120;i=i+2) { inst[i]=0x06; inst[i+1]=0xa2; } rt_com_write(0, &inst[0], 120); printk(KERN_INFO "get input sentx80\n"); break; case 0x74: //t turn and film // turn_motor_speed(0,-1,2500000000,10000000,1000000); //1/4 turn_motor_speed(0,-1,2050000000,10000000,1000000); //1/4 break; case 0x75: //u undo right step #if 0 //no friction turn_motor_speed_t(2,-1,0,20000000,2000000,1000000); //1/4 turn_motor_speed_t(3,1,0,20000000,2000000,1000000); //1/4 turn_motor_speed_t(8,-1,0,50000000,2000000,1500000); //1/4 //less on rt leg turn_motor_speed_t(9,1,0,30000000,2000000,1500000); //1/4 //less thrust on foot down (gravity makes more easy) turn_motor_speed_t(1,1,50000000,40000000,2000000,1000000); //1/4 //same for knee turn_motor_speed_t(5,-1,50000000,160000000,2000000,1000000); //1/4 //unlift leg! turn_motor_t(7,1,70000000,100000000); //never more than .4s!!! #endif #if 1 //with friction //lean //motor 2,3 8,9 feet turn_motor_speed_t(2, 1,0,200000000,M100,M50); //1/4 turn_motor_speed_t(3,-1,0,200000000,M100,M50); //1/4 //hip turn_motor_speed_t(8, 1,0,200000000,M100,M75); //1/4 turn_motor_speed_t(9, 1,0,200000000,M100,M75); //1/4 //step - right leg //motor 1 5 7,6 foot and knee 200ms turn_motor_speed_t(1, 1,200000000,50000000,M100,M50); //1/4 turn_motor_speed_t(5,-1,200000000,200000000,M100,M50); //1/4 //lift leg! 15 400ms turn_motor_t(1,-1,400000000,100000000); //never more than .4s!!! turn_motor_t(5, 1,400000000,200000000); //never more than .4s!!! turn_motor_t(7, 1,500000000,300000000); //never more than .4s!!! turn_motor_t(6, 1,500000000,300000000); //never more than .4s!!! #endif #if 0 //turn_motor_speed_t(2,-1,0,200000000,M100,M50); //1/4 //turn_motor_speed_t(3,1,0,200000000,M100,M50); //1/4 //hip turn_motor_speed_t(8,-1,0,200000000,M100,M75); //1/4 turn_motor_speed_t(9, 1,0,200000000,M100,M75); //1/4 //step - right leg //motor 1 5 7,6 foot and knee 200ms //turn_motor_speed_t(1,-1,200000000,50000000,M100,M50); //1/4 turn_motor_speed_t(5,-1,200000000,50000000,M100,M50); //1/4 //keep rt leg up when knee bends turn_motor_speed_t(9, 1,200000000,200000000,M100,M75); //1/4 //lift leg! 15 400ms //turn_motor_t(1, 1,400000000,100000000); //never more than .4s!!! //swing knee and leg forward together turn_motor_t(5, 1,400000000,50000000); //never more than .4s!!! turn_motor_t(7, 1,500000000,300000000); //never more than .4s!!! turn_motor_t(6, 1,500000000,300000000); //never more than .4s!!! #endif break; case 0x77: //w right step //1 right, -1 left opp for other motor //right arrow=-1 left arrow=1 //was using 1ms cycle, now 2ms cycle - too much memory is used with 1ms #if 0 //no friction //lean //motor 2,3 8,9 turn_motor_speed_t(2,1,0,20000000,2000000,1000000); //1/4 turn_motor_speed_t(3,-1,0,20000000,2000000,1000000); //1/4 turn_motor_speed_t(8,1,0,50000000,2000000,1500000); //1/4 turn_motor_speed_t(9,-1,0,50000000,2000000,1500000); //1/4 //step - right leg //motor 1 5 7,6 turn_motor_speed_t(1,-1,50000000,50000000,2000000,1000000); //1/4 turn_motor_speed_t(5,1,50000000,200000000,2000000,1000000); //1/4 //lift leg! 15 turn_motor_t(7,-1,70000000,120000000); //never more than .4s!!! #endif #if 1 //old step //with friction //lean //motor 2,3 8,9 feet turn_motor_speed_t(2,-1,0,100000000,M100,M50); //1/4 turn_motor_speed_t(3, 1,0,100000000,M100,M50); //1/4 //hip turn_motor_t(8,-1,0,200000000); //1/4 turn_motor_t(9,-1,0,200000000); //1/4 //turn_motor_speed_t(8,-1,0,300000000,M100,M75); //1/4 //turn_motor_speed_t(9,-1,0,300000000,M100,M75); //1/4 //step - right leg //motor 1 5 7,6 foot and knee 200ms turn_motor_speed_t(1,-1,210000000,50000000,M100,M50); //1/4 turn_motor_speed_t(5,-1,210000000,200000000,M100,M50); //1/4 //lift leg! 15 400ms //robot needs .5 second to shift weight turn_motor_t(1, 1,400000000,100000000); //never more than .4s!!! turn_motor_t(5, 1,400000000,500000000); //never more than .4s!!! turn_motor_t(7,-1,500000000,300000000); //never more than .4s!!! turn_motor_t(6,-1,500000000,300000000); //never more than .4s!!! turn_motor_speed_t(4, 1,500000000,200000000,M100,M50); //1/4 knee turn_motor_speed_t(0,-1,500000000, 50000000,M100,M50); //1/4 //turn_motor_speed_t(8,1,800000000,300000000,M100,M75); //1/4 //turn_motor_speed_t(9,1,800000000,300000000,M100,M75); //1/4 //shift weight at feet turn_motor_speed_t(2, 1,800000000,100000000,M100,M50); //1/4 turn_motor_speed_t(3,-1,800000000,100000000,M100,M50); //1/4 //should put 6,7 back also //put knee and foot back down to ground turn_motor_t(1, 1,900000000,100000000); //never more than .4s!!! turn_motor_t(5, 1,900000000,500000000); //never more than .4s!!! //put hip back- make sure has landed //turn_motor_t(8, 1,1400000000,200000000); //1/4 //turn_motor_t(9, 1,1400000000,200000000); //1/4 //this is where I put my foot down //turn_motor_speed_t(1,1,200000000,50000000,M100,M50); //1/4 //undo step //swing knee forward //turn_motor_speed_t(5,-1,600000000,200000000,2000000,1000000); //1/4 //turn_motor_speed_t(1, 1,600000000,50000000,2000000,1000000); //1/4 //undo waist //turn_motor_speed_t(8,1,800000000,200000000,2000000,1500000); //1/4 //turn_motor_speed_t(9,-1,800000000,200000000,2000000,1500000); //1/4 //undo feet //turn_motor_speed_t(2, 1,1000000000,500000000,2000000,1000000); //1/4 //turn_motor_speed_t(3,-1,1000000000,500000000,2000000,1000000); //1/4 #endif #if 0 //new step //motor 2,3 8,9 feet //turn_motor_speed_t(2,-1,0,200000000,M100,M50); //1/4 //turn_motor_speed_t(3,1,0,200000000,M100,M50); //1/4 //hip turn_motor_speed_t(8, 1,0,200000000,M100,M75); //1/4 turn_motor_speed_t(9,-1,0,200000000,M100,M75); //1/4 //step - right leg //motor 1 5 7,6 foot and knee 200ms //turn_motor_speed_t(1,-1,200000000,50000000,M100,M50); //1/4 turn_motor_speed_t(5, 1,200000000,50000000,M100,M50); //1/4 //keep rt leg up when knee bends turn_motor_speed_t(9,-1,200000000,200000000,M100,M75); //1/4 //lift leg! 15 400ms //turn_motor_t(1, 1,400000000,100000000); //never more than .4s!!! //swing knee and leg forward together turn_motor_t(5,-1,400000000,50000000); //never more than .4s!!! turn_motor_t(7,-1,500000000,300000000); //never more than .4s!!! turn_motor_t(6,-1,500000000,300000000); //never more than .4s!!! #endif break; case 0x78: //x //leg lift pulse //turn_motor(curmotor,-1,400000000); //never more than .4s!!! turn_motor(curmotor,1,100000000); //never more than .4s!!! break; case 0x79: //y break; case 0x7a: //z turn_motor(curmotor,-1,100000000); //never more than .4s!!! break; default: //rt_com_write(0, &Key, 2); break; } //end switch } //end while err return(0); } //end my_handler #endif static void thread_code(int tsk) { int a; int numdat[10]; //Task // 0 = send get balance command // 1 = get balance data, send motor commands // 2 = send fifo get images command to user app while(1) { if (tsk==0) //send command to get adxl202 balance data { //80 at 115200 (8.681 us) = 694.4 us rt_com_write(0, &balsend0d[0], 2);//140); //140/80 for 1Mohm } if (tsk==1) //get adxl202 balance data { //#if 0 // numdat[0]=rt_com_read(0, &balget[0], 90); //90 if extra data in buffer //numdat[0]=rt_com_read(0, &balget0d[0], 80);//80); //+10 if extra data in buffer numdat[0]=rt_com_read(0, &balget0d[0], 2);//80); //+10 if extra data in buffer //process the balance data //actually read half as many bytes rt_printk( "0d:"); for(a=0;a<1;a++) { rt_printk( "%x ",balget0d[a]&0x1f);//&0x1f); } //rt_printk( "%s",balget0a);//[a]); if (0 && rt_get_time_ns()>nextpulse0d) { //compare to last pulse rt_printk( "\nmotor torso:1 1:+1"); //turn_motor_speed_t(6,1,0,100000,M100,M50); //1ms //turn_motor_speed_t(7,-1,0,100000000,M100,M1); //100ms//20ms // turn_motor(6, 1,20000000); //10ms // turn_motor(7,-1,20000000); //10ms nextpulse0d=rt_get_time_ns()+100000000; //100ms } if (0 && rt_get_time_ns()>nextpulse0d) { //compare to last pulse rt_printk( "\nmotor torso:-1 1:+1"); // turn_motor_speed_t(6,-1,0,100000000,M100,M1); //20ms // turn_motor_speed_t(7,1,0,100000000,M100,M1); //100ms//20ms // turn_motor(6,-1,20000000); //10ms // turn_motor(7, 1,20000000); //10ms nextpulse0d=rt_get_time_ns()+100000000; //100ms } //#endif } //if tsk==1 if (tsk==2) //send command to get adxl202 balance data { //80 at 115200 (8.681 us) = 694.4 us rt_com_write(0, &balsend0a[0], 2);//140); //140/80 for 1Mohm //average if 40 bytes + <20 for half cycle = always under 60 bytes (x2=120 inst) } if (tsk==3) //get adxl202 balance data { //#if 0 // numdat[1]=rt_com_read(0, &balget0d[0], 90); //90 if extra data in buffer numdat[1]=rt_com_read(0, &balget0a[0], 2);//80); //+10 if extra data in buffer //process the balance data //actually read half as many bytes rt_printk( " 0a:"); for(a=0;a<1;a++) { rt_printk( "%x\n",balget0a[a]&0x3f);//&0x1f); } //rt_printk( "%s",balget0a);//[a]); if (0 && rt_get_time_ns()>nextpulse0a) { //300,000ns * 20 = 6ms check every 50ms turn motor for at least 60ms rt_printk( "\nlegs:+1 3:-1"); // turn_motor_speed_t(0,1,0,100000000,M100,M1); //100ms // turn_motor_speed_t(1,-1,0,100000000,M100,M1); //100ms //turn_motor(0,1,20000000); //20ms //turn_motor(1,-1,20000000); //20ms nextpulse0a=rt_get_time_ns()+100000000; //100ms } if (0 && rt_get_time_ns()>nextpulse0a) { //compare to last pulse rt_printk( "\nlegs:-1 3:+1"); // turn_motor_speed_t(0,-1,0,100000000,M100,M1); //20ms // turn_motor_speed_t(1,1,0,100000000,M100,M1); //100ms//20ms //turn_motor(0,-1,20000000); //20ms //turn_motor(1,1,20000000); //20ms nextpulse0a=rt_get_time_ns()+100000000; //100ms } //#endif } //if tsk==3 rt_task_wait_period(); } //end while 1 } //end thread_code static void kbhandler(void) { //keyboard IRQ ISR unsigned char ky,kyc; unsigned long flags; RTIME rtm; unsigned char inst[10],gbyt; int err; ky=inb(0x60); #if 0 //rt_printk( "send fifo %d\n",ky); rtm=rt_get_time_ns(); rtf_put(0,&rtm,sizeof(RTIME)); rtf_put(0,&ky,1); #endif if (ky==129 || ky==1) //esc key { rtf_put(0,&ky,1); rt_free_global_irq(1); } //rt_printk( "%c\n",keymap[ky]); // rt_printk( "%d\n",ky); //process keypress switch(ky) { case 2: //1 curmotor=1; curmotor=768; break; case 3: //2 curmotor=2; curmotor=49; break; case 4: //3 curmotor=769; break; case 5: //4 curmotor=50; break; case 6: //5 curmotor=770; break; case 7: //6 curmotor=51; break; case 8: //7 curmotor=771; break; case 9: //8 curmotor=52; //curmotor=53; break; case 10: //9 curmotor=832; break; case 11: //0 curmotor=0; curmotor=48; break; case 23: //i get input inst[0]=0x02; inst[1]=0x00; inst[0]=0x10; inst[1]=0x00; inst[0]=0x00; inst[1]=0x00;//1; rt_com_write(0, &inst[0], 2); err=rt_com_read(0, &gbyt, 1); rt_printk("input= %d %x\n",err,gbyt); break; case 77: //-> turn_motor(curmotor,-1,10000000); //pulse motor 0 dir 1 for 10ms break; case 75: //<- turn_motor(curmotor,1,10000000); //pulse motor 0 dir 1 for 10ms break; }//end switch rt_pend_linux_irq(1); //send IRQ to Linux } //end kbhandler int init_module(void) { int ret; RTIME curtime; RTIME lasttime; int a,b; char buffer[256]; int count; for(a=0;a<200;a=a+2) { #if OLDBALANCE balsend0d[a]=0x06; balsend0d[a+1]=0x80; #else // balsend[a]=0x06; //0110 // balsend[a+1]=0x4; //10 0100 balsend0a[a]=0x06; //0110 //bottom 1011 balsend0a[a+1]=0xc0; //11* 1100 balsend0d[a]=0x07; //0111 //top problem with 1100 balsend0d[a+1]=0x0; //12* 0000 // balsend[a]=0x07; //0111 // balsend[a+1]=0x4; //13 0100 #endif } balance=0; ret=rt_com_hwsetup(0,0,0); if (ret <0) { if (ret==-16) { printk(KERN_INFO "ttyS0 port busy\n"); } else { rt_printk( "rt_com test: problems with ttyS0 hardware setup %d\n",ret ); } return( -1 ); } ret=rt_com_setup(0,115200,0,RT_COM_PARITY_NONE,1,8,-1); //ret=rt_com_setup(0,57600,0,RT_COM_PARITY_NONE,1,8,-1); if (ret<0) { rt_printk( "rt_com test: problems with setup\n" ); return( -1 ); } //do a read to empty read buffer of any (usually 2 bytes) count = rt_com_read( 0, buffer, 255 ); //make fifos and keyboard fifo handler function rtf_create(0, 100); //make rt fifo //rtf_create_handler(0, X_FIFO_HANDLER(my_handler)); rtf_create(1, 100); //make rt fifo //rtf_create_handler(1, X_FIFO_HANDLER(my_handler)); //make send balance get input commands task [0] rt_task_init(&tasks[0],thread_code,0,1000,0,0,0); //make get balance data and send motor task [1] rt_task_init(&tasks[1],thread_code,1,1000,0,0,0); rt_task_init(&tasks[2],thread_code,2,1000,0,0,0); rt_task_init(&tasks[3],thread_code,3,1000,0,0,0); //not sure difference between oneshot and periodic //I think oneshot uses CPU to time events/threads, not timer rt_set_oneshot_mode(); start_rt_timer_ns(1000000); a=0; while (a<23) { lasttime=rt_get_cpu_time_ns(); // printk(KERN_INFO "%li\n",lasttime); //printk (KERN_INFO "%c",hello[a]); // rt_com_write(sport,&hello[a],1); a++; // for(b=0;b<30000;b++) {} } pictasknum=0; for (a=0;a<100;a++) { picport[a]=0; } getimages=0; curmotor=0; bav[0]=0; bav[1]=0; tav[0]=0; tav[1]=0; akal0a=-22;//0;//-13;//-20;//bottom (torso) (wire #2) blue (not yel) akal0d=-25;//0;//-25;//-60; //top (legs) (wire #1) blue (not tape) nextpulse0a=rt_get_time_ns(); nextpulse0d=rt_get_time_ns(); calibrate=0; for(a=0;a<10;a++) { tavm0d[a]=0; tavm0a[a]=0; } rt_request_global_irq(1,kbhandler); //get keyboard interrupt return( 0 ); } void cleanup_module(void) { int count; char buffer[ 300 ]; int a; int sport; int tmp; int avea,aveb; sport=0; #if 0 count = rt_com_read( sport, buffer, 255 ); buffer[count ] = 0; rt_printk( "rt_com test: %d characters read: \"%s\"\n", count, buffer ); avea=0; aveb=0; for (a=0;a>1); tmp=(int)buffer[a]; // tmp[1]=0; //rt_printk( " %x ",buffer[a]); } // avea/=count; // aveb/=count; rt_printk( "\nave= %d/%d %d/%d ",avea,count,aveb,count); #endif rtf_destroy(0); //destroy fifo /dev/rtf0 rtf_destroy(1); //destroy fifo /dev/rtf1 stop_rt_timer(); rt_task_delete(&tasks[0]); rt_com_setup( 0, -1, 0, 0, 0, 0,0 ); rt_com_setup( 1, -1, 0, 0, 0, 0,0 ); rt_com_hwsetup( 0, -1, 0 ); rt_com_hwsetup( 1, -1, 0 ); //#endif rt_free_global_irq(1); rt_printk( "rt_com test: finished\n"); }