#define __KERNEL__
#define __RT__
#define MODULE

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/version.h>

#include <rtai.h>
#include <rtai_sched.h>
#include <rtai_fifos.h>
#include <rt_com.h>

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<<mb);  //add motor direction to data
}
else
{
data=(0x2<<mb);  //add motor direction to data
}


inst=(~((0x3<<mb)<<0x10))&0x00ff0000;
inst|=((addr<<0x8)|data);


inst2=(~((0x3<<mb)<<0x10))&0x00ff0000;
inst2|=(addr<<0x8);


knt=0;

//for (a=0;a<dur;a+=cycle)
//need to delay or else the commands will delay "now"
//I probably should be using a more absolute time reference
//for example at exactly some time do the command, not relative to now
//delay for .01 second for processing
for (a=10000000;a<(dur+10000000);a+=cycle)
  {
  rt_task_init(&pictask[pictasknum],picthread_code,inst,TMEM,0,0,0);
  rt_task_make_periodic_relative_ns(&pictask[pictasknum],a,0);
  pictasknum++;
  if (pictasknum>=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<<mb);  //add motor direction to data
}
else
{
data=(0x2<<mb);  //add motor direction to data
}


inst=(~((0x3<<mb)<<0x10))&0x00ff0000;
inst|=((addr<<0x8)|data);


inst2=(~((0x3<<mb)<<0x10))&0x00ff0000;
inst2|=(addr<<0x8);


knt=0;

//for (a=0;a<dur;a+=cycle)
//need to delay or else the commands will delay "now"
//I probably should be using a more absolute time reference
//for example at exactly some time do the command, not relative to now
//delay for .01 second for processing
for (a=10000000+st;a<(dur+10000000+st);a+=cycle)
  {
  //memory allocated here can be a problem - more than 256 =
  //rt_free_sysrq_handler() message
  rt_task_init(&pictask[pictasknum],picthread_code,inst,TMEM,0,0,0);
  rt_task_make_periodic_relative_ns(&pictask[pictasknum],a,0);
  pictasknum++;
  if (pictasknum>=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<<mb);  //add motor direction to data
}
else
{
data=(0x2<<mb);  //add motor direction to data
}


inst=(~((0x3<<mb)<<0x10))&0x00ff0000;
inst|=((addr<<0x8)|data);


inst2=(~((0x3<<mb)<<0x10))&0x00ff0000;
inst2|=(addr<<0x8);


knt=0;

//for (a=0;a<dur;a+=cycle)
//need to delay or else the commands will delay "now"
//I probably should be using a more absolute time reference
//for example at exactly some time do the command, not relative to now
//delay for .01 second for processing
for (a=10000000;a<(dur+10000000);a+=cycle)
  {
  rt_task_init(&pictask[pictasknum],picthread_code,inst,TMEM,0,0,0);
  rt_task_make_periodic_relative_ns(&pictask[pictasknum],a,0);
  pictasknum++;
  if (pictasknum>=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<<mb);  //add motor direction to data
}
else
{
data=(0x2<<mb);  //add motor direction to data
}



if (dir!=0)
{
inst=(~(0x3<<mb)<<0x10)&0x00ff0000;
inst|=((addr<<0x8)|data);

//rt_printk( "\nset motor inst=%x",inst);


rt_task_init(&pictask[pictasknum],picthread_code,inst,TMEM,0,0,0);
rt_task_make_periodic_relative_ns(&pictask[pictasknum],0,0);
pictasknum++;
if (pictasknum>=MAXPICTASK)
{
pictasknum=0;
}

} //end if dir!=0
else
{  //send stop
inst=(~(0x3<<mb)<<0x10)&0x00ff0000;
inst|=(addr<<0x8);

//inst = 00110000 00000000 00000000     shift addr data

rt_task_init(&pictask[pictasknum],picthread_code,inst,TMEM,0,0,0);
rt_task_make_periodic_relative_ns(&pictask[pictasknum],0,0);
pictasknum++;
if (pictasknum>=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<<mb);  //add motor direction to data
}
else
{
data=(0x2<<mb);  //add motor direction to data
}

if (dir!=0)
{
inst=((~(0x3<<mb))<<0x10)&0x00ff0000;
inst|=((addr<<0x8)|data);

//rt_printk( "\nmotor inst=%x\n",inst);


rt_task_init(&pictask[pictasknum],picthread_code,inst,TMEM,0,0,0);
//rt_task_make_periodic_relative_ns(&pictask[pictasknum],0,0);
rt_task_make_periodic_relative_ns(&pictask[pictasknum],0,-1);
pictasknum++;
if (pictasknum>=MAXPICTASK)
{
pictasknum=0;
}

} //end if dir!=0
inst=((~(0x3<<mb))<<0x10)&0x00ff0000;
inst|=(addr<<0x8);


//take out to test data lines
rt_task_init(&pictask[pictasknum],picthread_code,inst,TMEM,0,0,0);
rt_task_make_periodic_relative_ns(&pictask[pictasknum],dur,0);
pictasknum++;
if (pictasknum>=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<<mb);  //add motor direction to data
}
else
{
data=(0x2<<mb);  //add motor direction to data
}

if (dir!=0)
{
inst=((~(0x3<<mb))<<0x10)&0x00ff0000;
inst|=((addr<<0x8)|data);

//rt_printk( "\nmotor inst=%x\n",inst);


rt_task_init(&pictask[pictasknum],picthread_code,inst,TMEM,0,0,0);
//rt_task_make_periodic_relative_ns(&pictask[pictasknum],0,0);
rt_task_make_periodic_relative_ns(&pictask[pictasknum],st,-1);
pictasknum++;
if (pictasknum>=MAXPICTASK)
{
pictasknum=0;
}

} //end if dir!=0
inst=((~(0x3<<mb))<<0x10)&0x00ff0000;
inst|=(addr<<0x8);

rt_task_init(&pictask[pictasknum],picthread_code,inst,TMEM,0,0,0);
//rt_task_make_periodic_relative_ns(&pictask[pictasknum],dur,0);
rt_task_make_periodic_relative_ns(&pictask[pictasknum],st+dur,0);
pictasknum++;
if (pictasknum>=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<count;a++)
       {
       avea+=buffer[a]&0x1;
       aveb+=((buffer[a]&0x2)>>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");
}