/*********************************************
This program was produced by the
CodeWizardAVR V1.0.2.1 Standard
Automatic Program Generator
© Copyright 1998-2001
Pavel Haiduc, HP InfoTech S.R.L.
http://infotech.ir.ro
e-mail:dhptechn@ir.ro , hpinfotech@xnet.ro

Project : neobot
Version : 1.0
Date    : 10/14/2001
Author  : Theodore Johnson                
Company : New York USA                    
Comments: 
	Test out software speed control.

Chip type           : AT90S8535
Clock frequency     : 4.000000 MHz
Memory model        : Small
Internal SRAM size  : 512
External SRAM size  : 0
Data Stack size     : 128
*********************************************/

#include <90s8535.h>

// Standard Input/Output functions
#include <stdio.h>
#include <delay.h>
#include<math.h>   
                                                 

/////////////////////////////////////////////////
// IO Pin Definitions 

#define PHOTO_R PINA.6  	// bus 0
#define PHOTO_L PINA.7 		// bus 7
#define SPKR PORTC.7		// bus 8
#define BMPR_R PINC.6		// bus 9
#define BMPR_L PINC.5		// bus 10
#define LED_L PORTC.4		// bus 11
#define MOTOR_L PORTC.3		// bus 12
#define MOTOR_R PORTC.2		// bus 13
#define BTN PINC.0			// bus 14
#define LED_R PORTC.1		// bus 15

#define IRLED PORTB.0		// bus 2
#define IRDETECT PINB.1		// bus 1   

#define ENCODER_R PIND.3	// INT1
#define ENCODER_L PIND.2	// INT0

//			For quadrature test
#define QUAD0intr PIND.2
#define QUAD0dir PIND.4
#define QUAD1intr PIND.3
#define QUAD1dir PIND.5


/////////////////////////////////////
//		processor speed, clock tick rates
//		ensure that this is consistent with INIITIALIZATION OVERRIDES
#define XTAL 4000000L

#define CLOCK1_TICK 64
#define TICK1_PER_USEC 16	// must be (1000000/XTAL)*CLOCK1_TICK
#define LOG_TICK1_PER_USEC 4	// for fast divide.

#define CLOCK0_OVF 8*256


//////////////////////////////////////////////////
///		Allocate some space for variables to
///		communicate with assumble routines. 
///		THIS SHOULD BE THE 1ST VARIABLE DEFINITION.

unsigned char reserved_space[16];

//////////////////////////////////////////////////
//		Bit variables
bit use_photocell;



//////////////////////////////////////////////////
//		Button debounce

unsigned char button_pressed(unsigned char b, unsigned char *s){
	unsigned char retval;
	
	if(b == 1 && *s == 0) retval = 1;
	else retval = 0;
	
	*s = ((*s) << 1) + b;
	return(retval);
}



//////////////////////////////////////////////////
//		Tone generation.

void beep_lo(){   
 unsigned int cycle;
	for(cycle=0;cycle<300;cycle++){
		SPKR = 1;
		delay_us(1000);
		SPKR = 0;
		delay_us(1000);
	}
}


void beep_med(){
 unsigned int cycle;
	for(cycle=0;cycle<600;cycle++){
		SPKR = 1;
		delay_us(500);
		SPKR = 0;
		delay_us(500);
	}
}

void beep_hi(){
 unsigned int cycle;
	for(cycle=0;cycle<1200;cycle++){
		SPKR = 1;
		delay_us(250);
		SPKR = 0;
		delay_us(250);
	}
}


       

//////////////////////////////////////////////////
//		Read the Photocell.
//		(use ADC, and adjust levels).

flash unsigned char n_adc = 2;
unsigned int a_val[6]; 
unsigned char curr_channel = 0; 


void read_brighness(unsigned int *left_level, unsigned int *right_level){
unsigned long int tmp_long;

	ADCSR.6 = 1;
	delay_ms(20);
// 	printf("R: %4d %4d\n",1023-a_val[0], 1023-a_val[1]); 
	*right_level = 1023-a_val[0];
	tmp_long = 1023-a_val[1];
	tmp_long = (388*tmp_long) / 1000;
	*left_level = (1023-a_val[1]) + (int)(tmp_long) - 5;
}


#define ADC_VREF_TYPE 0x00
// ADC interrupt service routine
interrupt [ADC_INT] void adc_isr(void)
{
unsigned int adc_data;
// Read the ADC conversion result
	adc_data=ADCW;
	a_val[curr_channel] = adc_data;    // record the data
	curr_channel = (curr_channel+1)%n_adc; // next channel.
	ADMUX=ADC_VREF_TYPE | curr_channel;	// set the mux to the next channel   
	if(curr_channel > 0)
		ADCSR.6 = 1;	// enable the next conversion. 
		
}





/////////////////////////////////////////////////////
//		Motion control using timer interrupts to
//		generate PWM for the servos.

//			PWM for the servos
//			On0_width, etc, is the duration of the on/off pusle
//			width, in timer1 clock ticks (currently 16 us).
//			remaining0 : time until next interrupt for 0 (1)
//			remaining, save_sreg : pre-allocate some space save a push/pop
//			cycle1 : combo control and status register.
//				bit 0 : next transition for left motor (0 => hi)
//				bit 1 : next transition for right motor (0 => hi)
//				bit 4 : set => left motor enabled.
//				bit 5 : set => right motor enabled.

//			THE MEMORY SPACE INTs SHOULD LIE IN reserved_space  
//			MAKE CERTAIN THIS IS CONSISTENT WITH ASM CODE

unsigned int on0_width @0xE0;
unsigned int off0_width @0xE2;
unsigned int on1_width @0xE4;
unsigned int off1_width @0xE6;       

register unsigned char cycle @8;
register unsigned int remaining @9;
register unsigned int remaining0 @11;
register unsigned int remaining1 @13;
register unsigned char save_sreg @7;


//		pulse width control functions

void enable_pulse_left(){ 
#asm("cli")
	cycle = cycle | 0x10;
#asm("sei")	
}

void enable_pulse_right(){
#asm("cli")
	cycle = cycle | 0x20;
#asm("sei")	
}

void disable_pulse_left(){ 
#asm("cli")
	cycle = cycle & 0xEF;
#asm("sei")	
}

void disable_pulse_right(){
#asm("cli")
	cycle = cycle & 0xDF;
#asm("sei")	
}


//		Set pulse width in usec, convert to clock 1 ticks (round down)
void set_left_pulse(unsigned  int on_pulse, unsigned long int off_pulse){
	on0_width = (on_pulse >> LOG_TICK1_PER_USEC);
	off0_width = (off_pulse >> LOG_TICK1_PER_USEC);
}

void set_right_pulse(unsigned  int on_pulse, unsigned long int off_pulse){
	on1_width = (on_pulse >> LOG_TICK1_PER_USEC);
	off1_width = (off_pulse >> LOG_TICK1_PER_USEC);
}




//		Timer 1 IRS, generate PWM

#pragma savereg-
  
interrupt [TIM1_OVF] void tim1_isr(void){  

#asm
;			MAKE CERTAIN THIS IS CONSISTENT
;			WITH THE VARIABLE DECLARATIONS
.equ on0_width = 0xE0
.equ off0_width = 0xE2
.equ on1_width = 0xE4
.equ off1_width = 0xE6

.def cycle = r8
.def remaining = r9
.def remaininghi = r10
.def remaining0 = r11 
.def remaining0hi = r12
.def remaining1 = r13
.def remaining1hi = r14
.def save_sreg = r7

.def cycle_tmp = r16

.equ TCNT1L = 0x2c
.equ TCNT1H = 0x2d  
; .EQU SREG=0x3F
.equ MOTOR_PORT = 0x15
.equ MOTOR_LEFT = 3
.equ MOTOR_RIGHT = 2


	in save_sreg, SREG
	push r16
	push r17
              
	mov cycle_tmp, cycle	;	To use immediate instructions on cycle
              
; if(remaining0 == 0){	
	tst remaining0hi
	brne finished_0
	tst remaining0
	brne finished_0
	
;    if(cycle0 == 0){
	  sbrc cycle_tmp, 0
	  rjmp cycle0_eq_1

;		if(enable0)	  
;			PORTB.0 = 1	  
;		remaining0 = on0_width;
;		cycle0 = 1
		sbrc cycle_tmp, 4
        sbi MOTOR_PORT, MOTOR_LEFT
        lds remaining0, on0_width
        lds remaining0hi, on0_width+1
        sbr cycle_tmp, 1
        rjmp finished_0

cycle0_eq_1:
;	}else{
;		PORTB.0 = 0
;		remaining0 = off0_width
;		cycle0 = 0 
        cbi MOTOR_PORT, MOTOR_LEFT
        lds remaining0, off0_width
        lds remaining0hi, off0_width+1 
        cbr cycle_tmp, 1  
;	}
;  }

finished_0:


; if(remaining1 == 0){	
	tst remaining1hi
	brne finished_1
	tst remaining1
	brne finished_1
		
;    if(cycle1 == 0){
	  sbrc cycle_tmp, 1
	  rjmp cycle1_eq_1

;		if(enable1)	  
;			PORTB.1 = 1	  
;		remaining0 = on0_width
;		cycle1 = 1
		sbrc cycle_tmp, 5
        sbi MOTOR_PORT, MOTOR_RIGHT
        lds remaining1, on1_width
        lds remaining1hi, on1_width+1 
		sbr cycle_tmp, 2
        rjmp finished_1

cycle1_eq_1:
;	}else{
;		PORTB.1 = 0
;		remaining1 = off1_width        
;		cycle1 = 0
        cbi MOTOR_PORT, MOTOR_RIGHT
        lds remaining1, off1_width
        lds remaining1hi, off1_width+1     
        cbr cycle_tmp, 2
;	}
;  }

finished_1:          
	mov cycle, cycle_tmp	;	restore cycle, free up r16

;  remaining = remaining0
	mov remaining, remaining0
	mov remaininghi, remaining0hi
		
;  remaining = min(remaining, remaining1)
	cp remaining, remaining1
	cpc remaininghi, remaining1hi
	brlt no_set_min1
	mov remaining, remaining1
	mov remaininghi, remaining1hi

no_set_min1:	

        
;		next_intr = (65536-remaining)+1
	ldi r16, 0xff
	ldi r17, 0xff
	sub r16, remaining
	sbc r17, remaininghi
	inc r16
	brne no_next_intr_hireg_incr
	inc r17
no_next_intr_hireg_incr:	
       
;		TCNT = next_intr
	out TCNT1H, r17
	out TCNT1L, r16

;		update remaining time to interrupt registers
;	remaining0 -= remaining
	sub remaining0, remaining
	sbc	remaining0hi, remaininghi
	
;	remaining1 -= remaining
	sub remaining1, remaining
	sbc	remaining1hi, remaininghi


;		return	
	pop r17
	pop r16
	out SREG, save_sreg
	
	reti
	

;		


#endasm  

}

#pragma savereg+


///////////////////////////////////////
//		Quadrature encoder count
int intr0_cnt, intr1_cnt;
#define TICKS_R intr1_cnt
#define TICKS_L intr0_cnt

interrupt [EXT_INT0] void ext_int0_isr(void)
{
	unsigned char tmp_mcucr;
	if(QUAD0intr == QUAD0dir)
		intr0_cnt++;
	else
		intr0_cnt--;
	
    tmp_mcucr = MCUCR  & 0xFC;
	if(QUAD0intr)
		tmp_mcucr |= 2;
	else
		tmp_mcucr |= 3;
	                  
	MCUCR = tmp_mcucr;
			
}

interrupt [EXT_INT1] void ext_int1_isr(void)
{
	unsigned char tmp_mcucr;
	if(QUAD1intr == QUAD1dir)
		intr1_cnt--;
	else
		intr1_cnt++;
	
    tmp_mcucr = MCUCR  & 0xF3;
	if(QUAD1intr)
		tmp_mcucr |= 0x08;
	else
		tmp_mcucr |= 0x0C;
	                  
	MCUCR = tmp_mcucr;
			
}



////////////////////////////////////////////
//		Heartbeat tick.

//		event counters
unsigned long int hbeat_count;
unsigned int motion_iter = 0;     

#define LEFT 0
#define RIGHT 1

struct motor_str{
	int setpoint;		// To where the wheel is supposed to move
	int speed;			// Rate of change in setpoint, per motion adjustment
	char move_to;		// set when moving specified distance
	int remaining;		// remaining adjustment to setpoint during moveto
	int pwr;            // power level, for monitoring.
} motor_state[2];




//			Direction defines.
#define FORWARDS 0
#define REVERSE 1

#define MIN_ENCODER_TICK 4		// min delay per encoder transition
								// (filter out transients)
								// (1/4MHZ)*CLOCK0_OVF*MIN_ENCODER_TICK = 2ms

#define STOP_THRESHOLD 2	// number of motion processing cycles with no motion
							// before declaring the wheel has stopped.
#define MIN_ERR_THRESHOLD 3	// 	must be off by this many ticks to oscillate
#define MOVE_THRESHOLD	16	// max number of ticks trying to stop after a move_to
#define MIN_MOVETO_ERR 3		// must be within this distance to stop.

//		off pulse, in usec
#define MIN_OFF_PULSE	22000	//	fastest setting
#define MAX_OFF_PULSE 	90000	// slowest setting
#define POWER_RANGE 256			// range of power settings for the servos
#define POWER_MIN_THRESHOLD 10	// below this, turn off servo
#define MAX_TICK_ERR 1			// acceptable error in positioning.
#define PULSE_PER_STEP 265		// (MAX_OFF_PULSE-MIN_OFF_PULSE)/POWER_RANGE

//		pulse widths for {forwards, reverse}
flash unsigned int on_pulse_l[2] = {2000,710};
flash unsigned int on_pulse_r[2] = {710,2000};

#define Kp 200		// scaling factor : err (in ticks) to power [0,POWER_RANGE),
					// will divide by 8.     


//		Initializer for motor state
void init_motor_state(char rl){
	struct motor_str *m;
	m = &(motor_state[rl]);
	
	m->setpoint = 0;
	m->speed = 0;
	m->move_to = 0;
	m->remaining = 0;
	m->pwr = 0;
}

void reset_motor_ctrs(){
	motor_state[LEFT].setpoint = 0; TICKS_L = 0;
	motor_state[RIGHT].setpoint = 0; TICKS_R = 0;
}

//		Set speed, in ticks per 1/10 second
void set_speed(int speed, char rl){
	struct motor_str *m;
	m = &(motor_state[rl]);

	if(speed == 0){	// request to stop.
		m->speed = speed;
		m->move_to = 0;		// abandon move-to.
		m->remaining = 0;	// abandon move_to 
		return;
	}

	
	m->speed = speed;
	m->move_to = 0;		// move indefinitely
}

	

void move_to_pos(unsigned char speed,  int distance, char rl){	
	struct motor_str *m;
	int tick_diff;   
	
	m = &(motor_state[rl]);

// 			Reset positioning, preserve error.
	switch(rl){
	case LEFT:
		tick_diff = TICKS_L - m->setpoint;
		TICKS_L = tick_diff;
		break;
	case RIGHT:
		tick_diff = TICKS_R - m->setpoint;
		TICKS_R = tick_diff;
		break;
	}
	m->setpoint = 0;
		
	m->move_to = 1;
	m->speed = speed;
	m->remaining = distance;
	if(m->remaining < 0) m->speed = -((int)speed);	


}
	

void adjust_setpoint(char rl){
	struct motor_str *m;
	
	m = &(motor_state[rl]);

//			Adjust setpoint
	if(!m->move_to){		// Moving indefinitely
		m->setpoint = (m->setpoint) + (m->speed);
	}else{				// moving a particular distance
		if(abs(m->remaining) > abs(m->speed)){
			m->setpoint = (m->setpoint) + (m->speed);
			m->remaining = (m->remaining) - (m->speed);
		}else{
			m->setpoint = (m->setpoint) + (m->remaining);
			m->remaining = 0;
			m->speed = 0; 
		}
	}
}

void update_motor_power(){
	int err;
	int pwr;
	unsigned long int off_pulse;
	unsigned char dir;
	struct motor_str *m;

	
			 

//			First process left wheel		      
	m = &(motor_state[LEFT]);
	err = m->setpoint - TICKS_L;
	pwr = (err*Kp)/8;
	if(pwr<0){ pwr=-pwr; dir=REVERSE;}else{dir=FORWARDS;}
	m->pwr = pwr;
LED_L = 0;		
	if(pwr>POWER_MIN_THRESHOLD){
		if(pwr>POWER_RANGE){
LED_L = 1;
			pwr = POWER_RANGE-1;
		}
		off_pulse = MIN_OFF_PULSE+((unsigned long int)((POWER_RANGE-1)- pwr))*PULSE_PER_STEP;
		enable_pulse_left();
		set_left_pulse(on_pulse_l[dir], off_pulse);
	}else{			// diff too small, turn off motor
		disable_pulse_left();
		m->pwr = 0;
	}
			

//			Next process right wheel		      
	m = &(motor_state[RIGHT]);
	err = m->setpoint - TICKS_R;
	pwr = (err*Kp)/8;
	if(pwr<0){ pwr=-pwr; dir=REVERSE;}else{dir=FORWARDS;}
	m->pwr = pwr;
LED_R = 0;		
	if(pwr>POWER_MIN_THRESHOLD){
		if(pwr>POWER_RANGE){
LED_R = 1;
			pwr = POWER_RANGE-1;
		}
		off_pulse = MIN_OFF_PULSE+((unsigned long int)((POWER_RANGE-1)- pwr))*PULSE_PER_STEP;
		enable_pulse_right();
		set_right_pulse(on_pulse_r[dir], off_pulse);
	}else{			// diff too small, turn off motor
		disable_pulse_right();
		m->pwr = 0;
	}			
}

interrupt [TIM0_OVF] void tim0_isr(void){ 

	hbeat_count++;  // update current time

	
	motion_iter ++;
	if(motion_iter == 125){	//  do this every 1/16 sec
		motion_iter = 0;

//			Adjust the setpoints
		adjust_setpoint(LEFT);
		adjust_setpoint(RIGHT);
		
		update_motor_power();
	}
	
}

       

void wait_until_stop(){ 
int last_ticks_l, last_ticks_r;
unsigned char ldone=0, rdone=0;
unsigned char iter = 0;
struct motor_str *ml;
struct motor_str *mr;

  ml = &(motor_state[LEFT]);
  mr = &(motor_state[RIGHT]);

  while((!ldone) || (!rdone)){
	ldone = (ml->speed == 0) && (ml->pwr == 0);
	rdone = (mr->speed == 0) && (mr->pwr == 0);
	  	
	delay_ms(10);
	iter++;
	if(iter == 50){
		iter = 0;
		printf("Ls%d t%d %4d p%5d \n",ml->setpoint, TICKS_L,TICKS_L - last_ticks_l,
												ml->pwr);
		printf("Rs%d t%d d%4d a%4d\n",mr->setpoint, TICKS_R,TICKS_R-last_ticks_r,
												mr->pwr);
		last_ticks_l = TICKS_L; last_ticks_r = TICKS_R;
    }
  }
  	disable_pulse_left();
  	disable_pulse_right();
	printf("Ls%d t%d %4d p%5d \n",ml->setpoint, TICKS_L,TICKS_L - last_ticks_l,ml->pwr);
		printf("Rs%d t%d d%4d a%4d\n",mr->setpoint, TICKS_R,TICKS_R-last_ticks_r,
												mr->pwr);
  
}


//////////////////////////////////////////////
//		Main program


void main(void){ 
unsigned char btn_state, state;
unsigned int iter;


// Declare your local variables here

// Input/Output Ports initialization
// Port A
PORTA=0x00;
DDRA=0x00;

// Port B
PORTB=0x00;
DDRB=0x01;

// Port C
PORTC=0x00;
DDRC=0x9E;

// Port D
PORTD=0x00;
DDRD=0x00;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Output Compare
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer 1 Stopped
// Mode: Output Compare
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
TCCR1A=0x00;
TCCR1B=0x00;
TCNT1H=0x00;
TCNT1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;  
///////////			timer 1 settings overridden below.

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer 2 Stopped
// Mode: Output Compare
// OC2 output: Disconnected
TCCR2=0x00;
ASSR=0x00;
TCNT2=0x00;
OCR2=0x00;

// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
GIMSK=0x00;
MCUCR=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x00;

// UART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// UART Receiver: On
// UART Transmitter: On
// UART Baud rate: 9600
UCR=0x18;
UBRR=0x19;

// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;

// ADC initialization
// ADC Clock frequency: 2000.000 kHz
ADMUX=ADC_VREF_TYPE;
ADCSR=0x8A;



///////////////////////////////////////////////////
/////////	INIITIALIZATION OVERRIDES
///		More setup to be done
//		before interrupts are enabled.

/////				Timer for servo pulses.
TCCR1A = 0x00;
TCCR1B = 0x03;	// scale by 64 (us ms per tick)
// TCCR1B=0x00;		// disable timer
TCNT1 = 0xF000;	// set current clock to start interrupts soon.

/////				Timer for heartbeat
TCCR0 = 0x02;	// scale by 8 : .5ms per interrupt (at 4Mhz).
TCNT0 = 0x00;

TIMSK = 0x05;	// enable timers 0 and 1 overflow interrupt.


/////			External interrupts
MCUCR = 0x0F;	// Interrupt on rising edge.
GIMSK=0xC0;		// enable external interrupts.


//		Set up the ADC
ADMUX=ADC_VREF_TYPE;
ADCSR=0x8A;
curr_channel = 0;

//		Set up the servo PWM
remaining0=remaining1 = 0;
cycle = 0x00;
on0_width = on1_width = 100;
off0_width = off1_width = 500;
    


//		put motor drivers into an quiescient state.
delay_ms(300);

TICKS_L = 0; TICKS_R = 0;
init_motor_state(LEFT); init_motor_state(RIGHT);



// Global enable interrupts
#asm("sei")
TICKS_L = 0; TICKS_R = 0;

wait_until_stop();
    
///////////////////////////////////////////////////////
///			Movement test

if(1){
beep_med();
delay_ms(2000);
printf("Testing.\n");

while(1){

  for(iter=0;iter<2;iter++){

	beep_lo();
   
	move_to_pos(1,150,LEFT);
	move_to_pos(1,150,RIGHT);

	wait_until_stop();

	LED_L = 1; LED_R = 1;	btn_state = 0;
	beep_med();
	
 	LED_L = 1; LED_R = 1;	btn_state = 0;	
	while(!button_pressed((!BMPR_L), &btn_state)) delay_ms(20);
	LED_L = 0; LED_R = 0;
	delay_ms(2000);
 	beep_lo();
	

	move_to_pos(1,31,LEFT);
	move_to_pos(1,-31,RIGHT);

	wait_until_stop();

	beep_med();	
	LED_L = 1; LED_R = 1;	btn_state = 0;	
	while(!button_pressed((!BMPR_L), &btn_state)) delay_ms(20);
	LED_L = 0; LED_R = 0;
	
  }
  
  beep_hi();
  reset_motor_ctrs();
}}



//////////////////////////////////////////////////
//		Test the encoders

iter=0;
btn_state=0;

TCCR0=0x00;		// disable speed control timer

while (1){      
	printf("Testing Intr1\n");
	while(1){
	    delay_ms(1);
		LED_R=QUAD1intr;
		LED_L=QUAD1dir;
		iter++;
		if(iter>=1000){
		    printf("count=%d\n",intr1_cnt);
	    	iter=0;
		}
		if(button_pressed(BTN, &btn_state)) break;
	}	
	printf("Testing Intr0\n");
	while(1){
	    delay_ms(1);
		LED_R=QUAD0intr;
		LED_L=QUAD0dir;
		iter++;
		if(iter>=1000){
		    printf("count=%d\n",intr0_cnt);
	    	iter=0;
		}
		if(button_pressed(BTN, &btn_state)) break;
	}	
}



    
/////////////////////////////////////////////////////////////////
//////////	Test speed control

TCCR0=0x00;		// disable speed control timer
state = 0;
set_left_pulse(2000, 19200);
set_right_pulse(710, 19200);
while(0){
	if(button_pressed((!BMPR_L), &btn_state)){
		state = 1-state;
	}


	if(state%2 == 0){
		LED_L = 0; LED_R = 1;
		disable_pulse_left(); enable_pulse_right();
	}else{
		LED_L = 1; LED_R = 0;
		enable_pulse_left(); disable_pulse_right();
	}

	
	if(BTN)
		break;     
	delay_ms(200);
}
          
state = 0;
iter = 0;
while(1){
	if(button_pressed((!BMPR_L), &btn_state)){
		state ++;
		TICKS_L = TICKS_R = 0;
		if(state == 14) state = 0;
	}
	
	switch(state){
	case 0:
		set_left_pulse(2000, 16000);
		set_right_pulse(710, 16000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 1; LED_R = 1;
		break;	
	case 1:
		set_left_pulse(2000, 20000);
		set_right_pulse(710, 20000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 0; LED_R = 0;
		break;
	case 2:
		set_left_pulse(2000, 24000);
		set_right_pulse(710, 24000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 1; LED_R = 1;
		break;
	case 3:
		set_left_pulse(2000, 32000);
		set_right_pulse(710, 32000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 1; LED_R = 0;
		break;
	case 4:
		set_left_pulse(2000, 48000);
		set_right_pulse(710, 48000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 0; LED_R = 1;
		break;
	case 5:
		set_left_pulse(2000, 64000);
		set_right_pulse(710, 64000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 1; LED_R = 1;
		break;
	case 6:
		set_left_pulse(2000, 80000);
		set_right_pulse(710, 80000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 0; LED_R = 0;
		break;
	case 7:
		set_left_pulse(2000, 96000);
		set_right_pulse(710, 96000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 1; LED_R = 0;
		break;
	case 8:
		set_left_pulse(2000, 112000);
		set_right_pulse(710, 112000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 0; LED_R = 1;
		break;
	case 9:
		set_left_pulse(2000, 128000);
		set_right_pulse(710, 128000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 1; LED_R = 1;
		break;
	case 10:
		set_left_pulse(2000, 144000);
		set_right_pulse(710, 144000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 0; LED_R = 0;
		break;
	case 11:
		set_left_pulse(2000, 160000);
		set_right_pulse(710, 160000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 1; LED_R = 0;
		break;
	case 12:
		set_left_pulse(2000, 176000);
		set_right_pulse(710, 176000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 0; LED_R = 1;
		break;
	case 13:
		set_left_pulse(2000, 192000);
		set_right_pulse(710, 192000);
		enable_pulse_left(); enable_pulse_right();
		LED_L = 1; LED_R = 1;
		break;
    }
	
	
	if(BTN)
		break;     
	delay_ms(1000); 
	
	iter++;
	if(iter == 2){
		iter = 0; 
		  
	    printf("%2d l=%5d, r=%5d\n",state,TICKS_L, TICKS_R);
	    TICKS_L = 0; TICKS_R = 0;
	 }
	    
		
}
LED_L = 0;
LED_R = 0;
	
///////	end test speed control
//////////////////////////////////////// 







}