/*
 * kabo1.c
 * Manual Control Program via terminal connection
 * c:\mc\cc51 %1 -ilp h=c:\mc m=s s=c52robot.asm  < need new startup file
 * that uses timer2 for PWM generation
 * Copyright (c) 2000 by Wichit Sirichote
 */

#include "c:\mc\8051io.h"  /* include i/o header file */
#include "c:\mc\8051reg.h"
#include "c:\mc\8051bit.h"

extern register char cputick;
extern register int  PWM1,PWM2;

register char i,j,temp,flag1,flag2,speed1,speed2,led,step,index;
unsigned register char command,buffer[5];
register int count1,counter1,counter2;

unsigned int power[] = {0x0000,0x0001,0x0101,0x0841,0x1111,0x1249,0x2949,0x2A93,
0x5555,0xD555,0xD5D5,0xEDB6,0xEEEE,0xF7BE,0xFEFE,0xFFFE,0xFFFF};

/* Power Bit Hexadecimal Level Pattern Value by Fred Martin for MIT's MiniBoard */
/* the output P1.7 and P1.6 are connected to inverter, thus the power level
   is then be in reverse order, i.e., $0000 is top speed while the $FFFF is
   lowest speed (motors are stopped), however, user can use -16 to 16, for full
   reverse and full forward in program, since motor1() and motor2() functions
   will provide the proper value for PWM1 and PWM2. see below;

        0  0000000000000000 $0000   <--- top
        1  0000000000000001 $0001
        2  0000000100000001 $0101
        3  0000100001000001 $0841
        4  0001000100010001 $1111
        5  0001001001001001 $1249
        6  0010100101001001 $2949
        7  0010101010010101 $2A93
        8  0101010101010101 $5555
        9  1101010101010101 $D555
        10 1101010111010101 $D5D5    
        11 1110110110110110 $EDB6
        12 1110111011101110 $EEEE
        13 1111011110111110 $F7BE
        14 1111111011111110 $FEFE
        15 1111111111111110 $FFFE
        16 1111111111111111 $FFFF    <--- stop 

*/

/* constants definition */

#define LED P1.3
#define blink_every 10
#define right_period 10
#define left_period 10


#define forward_speed 15
#define back_speed -15

#define SCLK P3.5
#define SDA P3.4


main()

{
   flag1 = 0;
   flag2 = 0;
   index = 0;
   stop_both();
   clrbit(LED)
   counter1 = 0;
   counter2 = 200;
   printf("\n KABO's Manual Control Program\n\
           \n command keys: i,j,k,m or Arrows, p (print ADC0,1,2,3,port_B)\
           \n\
           \n             I (forward)\
           \n (left)  J       K (right)\
           \n             M (backward)\
           \n\
           \n SPACE bar (stop)\n");

   while(1)
   {
      while( cputick <10)
      ;
      cputick = 0;
// execute following tasks every 100ms
      command = chkch();  // soon emitted from sensoring decision
 //     command_emitter();
      print_ADC();
      cpubeat();
      forward();
      backward();
      turn_right();
      turn_left();
      stop();
      run_motor();
      timer();

   }
}

/*   robot_behaviors
     sleep();
     forward();
     backward();
     stop();
     forward_left();
     backward_right();
     backward_left();

     sensor tasks
     follow_line();
     seeking_line();
*/


/* motor1(int s)  s -16 to 16 */

motor1(int s)
{
     if (s < 0)
       setbit(P1.5)
       else clrbit(P1.5)
       clrbit(IE.7)
       PWM1 = power[16-abs(s)];
       setbit(IE.7)
}


/* motor2(int s)  s -16 to 16 */

motor2(int s)
{
    if (s < 0)
      setbit(P1.4)
      else clrbit(P1.4)
      clrbit(IE.7)
      PWM2 = power[16-abs(s)];
      setbit(IE.7)
}

run_motor(){

    if((flag1 & 0x01) != 0){
          stop_both();
          delay(3);
          motor1(speed1);
          motor2(speed2);
          flag1 &= ~0x01;
          flag2 |= 0x01;   // run timer
          }
}

timer()
{
    if((flag2 & 0x01) != 0){
        count1--;
        if(count1 == 0)
           {
           stop_both();
           flag2 |= 0x80;   // timer done flag
           flag2 &= ~0x01;   // disable entering timer execution
           }
                            }
}

stop_both()
{
    motor1(0);
    motor2(0);
}

forward()
{
   if(command == 'i' || command == 0x0b){
   count1 = 500;
   speed1 = forward_speed-1;
   speed2 = forward_speed;
   flag1 |= 0x01;  // run forward
                     }
}

backward()
{
   if(command == 'm'|| command == 0x16){   
   count1 = 500;
   speed1 = back_speed;
   speed2 = back_speed;
   flag1 |= 0x01;  // run motor
                     }
}

stop(){
   if (command == ' '){
   stop_both();
   }
}

turn_right(){
  if(command == 'k'|| command == 0x0c){ 
   count1 = right_period;
   speed1 = 13;
   speed2 = -13;
   flag1 |= 0x01; // run motor
   }
}

turn_left(){
  if(command == 'j'|| command == 0x08){ 
   count1 = left_period;
   speed1 = -13;
   speed2 = 13;
   flag1 |= 0x01; // run motor
   }
}

cpubeat()
{
    beat5sec();
    livecpu();

}

beat5sec()  /* set bit P1.3 every counter1 > blink_every */

{
       counter1++;
        if (counter1 > blink_every )
        {
           counter1 = 0;
           flag1 |= 0x40; /* set bit 6 of flag1 to signal livecpu task */
           setbit(LED) 
           led = 1;      /* 3*100ms = 300ms LED on */
        }

}

livecpu()

{
    if ((flag1 & 0x40) != 0)
      {
        if (led == 0)
           {
           clrbit(LED) 
           flag1 &= ~0x40;
           }
           led--;
    }
}

char sequence[5] = {'i','j','i','j',' '}

command_emitter(){
    counter2--;
    if (counter2 < 0){
      counter2 = 300;
      if (index < 5)
      command = sequence[index++];
      else (index=0);
      putch(command);
      }
}

pause(int j)
{
   int i;
   for (i = 0; i < j; i++)
   ;
}

read_ADC()  //use rrc instruction shifting SDA in 40-bit register
{
        disable();
//        setbit(LED)
        clrbit(SCLK)       // start bit
        pause(10);
        for(i=0;i<40;i++)    /* loop 40-bit */
        {
         clrbit(SCLK)
         pause(3);
         setbit(SCLK)        /* produce rising edge pulse */
         pause(3);
       asm{          // 40-bit SIPO register using buffer[5]
       setb  SDA
       mov   C,SDA
       mov   A,buffer+4
       rrc   A
       mov   buffer+4,A
       mov   A,buffer+3
       rrc   A
       mov   buffer+3,A
       mov   A,buffer+2
       rrc   A
       mov   buffer+2,A
       mov   A,buffer+1
       rrc   A
       mov   buffer+1,A
       mov   A,buffer
       rrc   A
       mov   buffer,A
       }
         }
//       clrbit(LED)
       enable();
}

print_ADC(){
   if (command == 'p'){
     read_ADC();
     putch(13);
     printf("                        ");
     putch(13);
     printf("%u %u %u %u %08b",buffer[0],buffer[1],buffer[2],buffer[3],buffer[4]);
   }
}