//file ATservoTB67S128UNO.c
//step nema 17 stepper, Toshiba chip driver using step and direction control
//runs on arduino uno atmega328 2k ram 16mhz 115200 baud
//----------------------------------
//Caravan Throttle quadrant has 5 pushpull cables 
//Autothrottle installation adds a pushrod to with a tab the same size as the lever, 
//The cable goes from throttle quadrant to factory destination, like fuel control in engine compartment
//The pushrod goes to the tab on the servo servo, which actuates cable back to throttle quadrant just like the lever,
//uses can
   
//Each servo:
//28V aircraft power
//stepper motor is kollmorgen nema17 PMX1750 4 wire, 200 step, 1.8 deg/step
//4 wire bipolar stepper and Toshiba TB67S128 stepper driver with step and direction (pololu bd has this chip)
//stepper driver enclosure houses clutch, position pot. This version uses UNO, target version uses AT90CAN32, debug uses local pot for demand
//stepper driver rod actuates cable just like throttle, same throw and force
//debug version used keyboard to issue step and direction cmds.
//release receives throttle quadranr lever position demand over can from flight control computer,
//driver reads position pot, reports back position from feedback pot to flight control computer on CAN 

//revisions:
//May 7 2022 Bob G initial edit step and direction version using toshiba stepper driver ic
//May 31 2022 Bob G
//June 1 2022 Bob G tab rotates 240 deg. Save offset from ccw stop as a cal param
//June 4 2022 Bob G

//todo:
//open loop step
//pot cal loop with raw, avg, volts,deg,steps, params in eeprom
//set driver chip discretes

//-------includes--------------------
#include <iccioavr.h> //mega328 io
#include <AVRdef.h>   //stackcheck
#include <stdio.h>    //cprintf
#include <stdlib.h>   //abs,rand,atoi
#include <string.h>   //memcpy
//#include <math.h>     //log
#include <ctype.h>    //toupper
#include <eeprom.h>

//------defines------------------------
#define INTR_OFF() asm("CLI")
#define INTR_ON()  asm("SEI")

//----struct used for rolling average------
#define NUMSAMP 16
typedef struct{
  int ndx;
  int tot;
  int avg;
  int dat[NUMSAMP]; //avg over NUMSAMP readings
}Trollavg;

//----------Xwing Autothrottle Servo types---------------
//addr sw:       0   1   2   3    4    5     6
enum servotype {EPL,PL1,PL2,PROP,FUEL,YTRIM,ETRIM};

//-----------------eeprom vars---------------------------
//#pragma data:eeprom  //default config params  
//unsigned char eeservotype=0; //0-7  
//#pragma data:data

//#define eesizebytes (&eeend-&eeneedleoffset)  //how do I print this out?

//------vars in bss-------
char junk;                //last var in bss
Trollavg rollavg[8];       //analog io
unsigned int addat[8];    //internal 10 bit
unsigned char dispon;
unsigned char del;  
unsigned char pos; 
unsigned int tics,ticsl;
unsigned int tmptics;
unsigned char servotype = 1; //0-6

//-----------code in flash rom---------
__flash unsigned char banner[]={"ATservoTB67S128UNO June 4 2022  q=quit\n"};

//----------------------
void port_init(void){
//mega328

  PORTB = 0x00; //pullups off
  DDRB  = 0xF8; //hi 5 all outs, lo 3 ins
 
  PORTC = 0x00; //a/ds
  DDRC  = 0x00; //all ins
 
  PORTD = 0x00; //pullups off
  DDRD  = 0xFC; //all outs except rx tx on lo bits
}

//--------------------
void uart0_init(void){
//UART0 initialize
// desired baud rate: 115200
// actual: baud rate:117647 (2.1%)
unsigned char jnk;

  jnk = UDR0;  //clear jnk char
  UCSR0B=0x00; //
  UCSR0A=0x02; //double speed
  UCSR0C=0x06; //8n1
  UBRR0H=0x00;
  UBRR0L=0x10; //115200 at 16
  UCSR0B=0x18; //0x18 = rx tx enab 0x98 = rx int enab
}

//#if 0
//---------------------
void spi_init(void){
//SPI initialisation

  SPCR = 0x50;   //setup SPI (53 slowest   50 fastest)
  SPSR = 0x01; //double speed
//  SPSR = 0x00;   //normal speed
}
//#endif

//--------------------
void timer0_init(void){
//TIMER0 initialize - mega328 uno
//16000000/64=250000   use divisor=250=0xfa gives 1ms interrupt

  TCCR0A = 0x00; //mode 0 normal
  TCCR0B = 0x03; //ps=3 divide by 64   16,000,000hz/64=250,000
  OCR0A  = 0xfa; //0xfa=250; 250000hz/250=1000hz=1ms       0x72=125 = 500us
  TIMSK0 = 0x02; //t0 compa int
}

#if 0
//--------------------------
void timer0_init(void){
//TIMER0 initialize - prescale:8   1ms real time
// WGM: Normal
// desired value: 1mSec

  TCNT0  = 0x00;
  TCCR0A = 0x00;
  TCCR0B = 0x03; //ctc mode
	
  OCR0A  = 0xfa;
//  OCR0B  = 0xfa;
	
  TIMSK0 = 0x02; //cha
//  TIFR0  = 0x00;
//  TCNT0  = 0x06;
}
#endif

//--------------------
void adc_init(void){
//ADC initialize

  ADCSRA = 0x00; //disable adc
  ADMUX  = 0x40; //select vcc and adc input 0
  ACSR   = 0x80; //disab analog comparator
  ADCSRA = 0x86;
}

//--------------------
void init_devices(void){
//mega328

  CLI(); //disable all interrupts
  port_init();
  spi_init();    //to init stepper driver chip
  uart0_init();
  adc_init();
  timer0_init(); //1ms timing

  MCUCR = 0x00;
  TIMSK0 = 0x02; //timer interrupt sources t0 compa
  SEI();         //re-enable interrupts
}

//------------------
void delnms(unsigned int n){
//delay n ms
int x;

  while(n--){
//    x=2400;       //empirically determined fudge factor  14 mhz
    x=2600;       //empirically determined fudge factor  16 mhz
//    x=2800;       //empirically determined fudge factor  18 mhz
//    x=3000;       //empirically determined fudge factor  20 mhz
    while(x--);
  }
}

#if 0
//------------------
void deln500us(int n){
//delay n ms
int x;

  while(n--){
    x=1300;       //empirically determined fudge factor  16 mhz
    while(x--);
  }
}
#endif

//--------------------
void initrollavg(Trollavg *p){
//init rollavg struct
char i;

  p->ndx=0;
  p->tot=0;
  p->avg=0;
  for(i=0; i < NUMSAMP; i++){
    p->dat[i]=0;
  }  
}

//--------------------
int calcrollavg(Trollavg *p, int w){
//return rollavg of last NUMSAMP readings

  p->tot -= p->dat[p->ndx]; //subtract old value from tot
  p->tot += w;              //add in new value
  p->dat[p->ndx++] = w;     //remember new value, bump ndx
  if(p->ndx == NUMSAMP) p->ndx=0; //rewind
  p->avg = p->tot >> 4;     //2^4 = 16
  return p->avg;
}

//-----------RS232----------------
unsigned char kbhit(void){
//return nonzero if char waiting  polled version
unsigned char b;

  b=0;
  if(UCSR0A & (1<<RXC0)) b=1;
  return b;
}

//-----------------------
int getchar(void){
//polled version... 
unsigned char c;

  while(!kbhit()){}; //wait for char
  c=UDR0;             //get char from usart
  return c;  
}

#if 0 //uncomment if rx int needed
//----------------------------
#define RXBUFSIZ 200
unsigned char rxbuf[RXBUFSIZ]; 
unsigned char rxindx,rxondx;   

//-------------------------
unsigned char kbhit(void){
//return nonzero if char waiting  intr version
unsigned char b;

  INTR_OFF();
  b=rxindx != rxondx;
	INTR_ON();
  return b;
}

//-----------------------
int getchar(void){
//intr version... overrides version in library
char c;

  while(!kbhit()){}; //wait for char
  c=rxbuf[rxondx++]; //get char from rxbuf
  if(rxondx >= RXBUFSIZ) rxondx=0;
  return c;  
}

//------------------------------
#pragma interrupt_handler uart0_rx_isr:iv_USART0_RX
void uart0_rx_isr(void){
//uart has received a character in UDR
char c;
unsigned int lrxindx;

  lrxindx=rxindx;
  c=UDR0;            //get char from usart
  rxbuf[lrxindx++]=c; //put char in rxbuf
  if(lrxindx >= RXBUFSIZ) lrxindx=0;
  rxindx=lrxindx;	  
}
#endif

//---------------------
int putc(char c){
//put char to usart0... dont add lf after cr

  while((UCSR0A & 0x20)==0){};	//0x20 is UDRE0. ==0 means data register full
  UDR0=c; //send char
  return c;
}

//----------------------  
int putchar(char c)	{
//adds lf after cr

  if(c=='\n'){
    putc('\r');
  }  
  putc(c);
  return c;
}

//-----serial utils---------------
void crlf(void){
//send cr lf

  putchar('\n');
}

//------------------
void space(void){
//send space

  putc(' ');
}
  
//------------------
void spaces(char n){
//send n spaces

  while(n--){
    space();
	}	
}
  
#define ESC 0x1b
//-------terminal utils---------------  
void gotoxy(unsigned char x, unsigned char y){
//ansi cursor positioning sequence E[y;xH

  putc(ESC);
  putc('[');
  cprintf("%d",y);
  putc(';');
  cprintf("%d",x);
  putc('H');
}

//----------------------
void clrscr(void){
//clear ansi terminal screen

  putc(ESC);
  putc('[');
  putc('2');
  putc('J');
}

//----------------------
void homecrsr(void){
//home cursor

  putc(ESC);
  putc('[');
  putc('f');
}

//------------------
void initscreen(void){
//clear screen and print banner

  clrscr();
	homecrsr();
  cprintf(banner);
}

//-------monitor utils------------------
unsigned char getche(void){
//get and echo a char from rs232
char c;

  c=getchar();
  putchar(c);
  return(c);
}

#if 0
//-----------------
void getstr(char *p){
//get a string and echo (used in cal menu)
char c;  

  do{
    c=getche();
    *p++=c;
  }while(c !=0x0d);
  p--;     //roll p back
  *p=0x00; //add null over cr
}
#endif

//-------monitor utils------------------
unsigned char gethex(void){
//return a hex char from rs232
unsigned char b,c;

  b=0xff; //error return value
  c=toupper(getche());
  if(isxdigit(c)) b=c-0x30; //if c ok, cvt ascii digit to binary
  if((c>='A') && (c<='F')) b-=7;     //if c hexcvt ascii A to binary 10
  return(b);
}

//---------------------------
unsigned char getbyte(void){
//get 2 nibbles, return a binary byte from rs232
unsigned char n1,n2;

  n1=gethex();
  n2=gethex();
  return((n1 << 4) + n2);
}

//----------------------------
unsigned int getaddr(void){
//return addr from rs232
unsigned int th,tl;

  th=getbyte();
  tl=getbyte();
  return((th << 8) + tl);
}

//------monitor subs-----------
void dump256ram(unsigned char *n){
//dump 16 rows of 16 bytes
unsigned char r,c;
unsigned char *p, *pa, ch;

  p=n;
  crlf();
  spaces(6);
  for(c=0; c < 16; c++){
    cprintf("%02x ",c);      //header
    if(c==7) space();
  }
  crlf();
  crlf();
  for(r=0; r < 16; r++){
    cprintf("%04x  ",p);     //print addr  at beg of line
    pa=p;                   //remember p for ascii
    for(c=0; c < 16; c++){
      cprintf("%02x ",*p++); //print hex
      if(c==7) space();
    }
    for(c=0; c < 16; c++){
      ch=*pa++;
      if((ch >= 0x20) && (ch <= 0x7f)) //if printing char
        putc(ch);   //print ascii
      else  
        putc('.'); 
      if(c==7) space();
    }
    crlf();
  }
}

//---------------------------------
void dump256rom(__flash unsigned char *n){
//dump 16 rows of 16 bytes
unsigned char r,c,ch;
__flash unsigned char *p, *pa;

  p=n;
  crlf();
  spaces(6);
  for(c=0; c < 16; c++){
    cprintf("%02x ",c);      //header
    if(c==7) space();
  }
  crlf();
  crlf();
  for(r=0; r < 16; r++){
    cprintf("%04x  ",p);     //print addr  at beg of line
    pa=p;                   //remember p for ascii
    for(c=0; c < 16; c++){
      cprintf("%02x ",*p++); //print hex
      if(c==7) space();
    }
    for(c=0; c < 16; c++){
      ch=*pa++;
      if((ch >= 0x20) && (ch <= 0x7f)) //if printing char
        putc(ch);   //print ascii
      else  
        putc('.'); 
      if(c==7) space();
    }
    crlf();
  }
}

//------------------
void examineram(void){
//ask for mem range and dump
unsigned char *from;
unsigned char c;

  cprintf("from:");
  from=(unsigned char *)getaddr();
  while(c!='q'){
    dump256ram(from);
    cprintf("np or q...");
    c=getchar();
    if(c=='n') from+=0x100;
    if(c=='p') from-=0x100;
  }  
}

//------------------
void examinerom(void){
//ask for mem range and dump
__flash unsigned char *from;
unsigned char c;

  cprintf("from:");
  from=(__flash unsigned char *)getaddr();
  while(c!='q'){
    dump256rom(from);
    cprintf("np or q...");
    c=getchar();
    if(c=='n') from+=0x100;
    if(c=='p') from-=0x100;
  }  
}

//------------------
void deposit(void){
//ask for addr and data
unsigned char *at;
unsigned char c;
unsigned char nh,nl;

  cprintf("at:");
  at=(unsigned char *)getaddr();
  while(1){
    cprintf(" %02x ",*at);
    nh=gethex();
    if(nh==0xff) return;
    nl=gethex();
    if(nl==0xff) return;
    c=((nh << 4) | nl);
    *at++=c;
  }
}

//------------------
void fill(void){
//ask for mem range and fill char and fill
unsigned char *from, *to, with;

  cprintf("from:");
  from=(unsigned char *)getaddr();
  cprintf(" to:");
  to=(unsigned char *)getaddr();
  cprintf(" with:");
  with=getbyte();
  memset(from,with,to-from);
}

//-----------------------------
void (* fn)(void); //fn prototype

void dojsr(void){
//ask for word addr, jsr to it
__flash unsigned char *to;

  cprintf(" to (word addr):");
  to=(__flash unsigned char *)getaddr();
  fn=(void *)to;
  (*fn)();  //call function fn and returns
}

//------------------
void debugmenu(void){
//monitor type cmds

  cprintf("cmds:\n"
          " e examineram\n"
          " c examinerom\n"
          " d deposit\n"
          " f fill\n"
          " j jsr\n"
          " q quit\n"
				);
}

//------------------
void debugloop(void){
//examine and deposit mem regs
char c;

  debugmenu();
  while(c != 'q'){
    crlf();
    cprintf("cmd:");
    c=getche();
    crlf();
    switch(c){
      case 'e': examineram(); break;
      case 'c': examinerom(); break;
      case 'd': deposit(); break;
      case 'f': fill();    break;
      case 'j': dojsr();   break;
      default: debugmenu();
    }
  }
}
//----end of debug subs----------
//#endif

//--------------------
void _StackOverflowed(char n){
//called from _Stackcheck()   must link this file first??

  INTR_OFF();
	if(n) putc('H'); else putc('S');
  while(1){};
}

__flash char spindat[4]={'-','\\','|','/'};
char spindex;
//------------------
void spin(void){

  spindex++;
  if(spindex > 3) spindex=0;
  cprintf("%c\r",spindat[spindex]);
}

//#if 0
//-----------------spi----------------------
char readspi(void){
//return char from spi
char cnt,stat;

  cnt=0;
  do{
    stat=SPSR;
    cnt++;
    if(cnt==255){
      cprintf("spi rcv to! ");
      break;
    }
  }while((stat & 0x80)==0);  //keep checking until done bit
  return(SPDR); //clears done flag
}

//--------------------------
void spiout8(char c){
//send c out spi
char cnt,stat;

  SPDR=c; //send data
  cnt=0;
  do{
    stat=SPSR;
	  cnt++;    
    if(cnt==255){
      cprintf("spi xmt to! ");
      break;
    }
  }while((stat & 0x80)==0);
}
//#endif

#define ADSCmsk 0x40
//---------------------
unsigned int readadchan(char n){
//read ch n of internal a/d  10 bit unsigned

  ADMUX= 0x40 + n;   //select vcc and channel n
  ADCSRA |= ADSCmsk; //init conversion
  while((ADCSRA & ADSCmsk) != 0){}; //wait for conv complete
  return ADC;
}

//#if 0
//--------------------
void readadloop(void){
//read internal 10 bit ad 0-0x3ff
char c,n;

  cprintf("a/d  q=quit\n");
  cprintf("0    1    2    3    4    5\n");
  c=0;
  while(c!='q'){
    if(kbhit()){
      c=getchar();
    }  
  	for(n=0; n < 6; n++){
	    addat[n]=readadchan(n);
      cprintf("%04d ",addat[n]);
	  }
    putc('\r');
  }
}
//#endif

#if 0
//--------------------------
void readeeprom(void){
//read eeprom params to ram

  INTR_OFF();
  EEPROMReadBytes((int)&eehr, (void *)&hr,1); 
  EEPROMReadBytes((int)&eemin, (void *)&min,1); 
  INTR_ON();
}
	
//--------------------------
void saveeeprom(void){
//save ram to eeprom

  INTR_OFF();
  EEPROMWriteBytes((int)&eehr, (void *)&hr,1); 
  EEPROMWriteBytes((int)&eemin, (void *)&min,1); 
  INTR_ON();
}
#endif

//---------------------
#pragma interrupt_handler timer0_compa_isr:iv_TIMER0_COMPA
void timer0_compa_isr(void){
//compare occured TCNT0=OCR0 1ms

  tics++;
//	putc('.');
}

//----------------------------------------------------------
//Xwing Autothrottle servo motor
//torque and speed req calcs
//given ratio of lever to tab example: pl 8.5" TO 1.3" (ratio 6:1),
//lever force of 2 lb with friction on makes tab force 12 lb up cable to fuel control. Much less with friction off.
//PL must move full 60 deg in .5 sec = 120 deg per sec. 60deg/1.8deg=33.3 steps in .5sec = 66.6/sec = 15ms/step
//torque on tab is 12 in-lb = 1 ft-lb = 192 in-oz
//hard stop at +-120deg on motor tab

//power is torque * speed. 1hp = 330 ft-lb /sec = 1/330 hp = 746/330= 2.2W,    
//other levers move full stroke in 2 sec, so they neec 1/4 the power, but same torque
//kollmorgen PMX1750 is 107 in-oz, ? ohms ? amps 1.8 deg/step 33.3 steps in 60 deg throw

//pl position 0-60 deg is sent from autopilot to auto throttle in percent 0-100
//position pot spanning 330 deg. 1023 lsbs/360deg =3.1 lsbs/deg 
//pot position in lsbs is converted to steps  lsbs/3.1=deg  deg*1.8=steps
  
//---pololu toshiba driver outs----
#define DIRCW()  PORTD |=  0x04
#define DIRCCW() PORTD &= ~0x04

#define STEPHI() PORTD |=  0x08
#define STEPLO() PORTD &= ~0x08
#define STEP() STEPHI(); NOP(); STEPLO()

#define ENABHI() PORTD |=  0x10
#define ENABLO() PORTD &= ~0x10

#define STBYHI() PORTD |=  0x20
#define STBYLO() PORTD &= ~0x20

#define RESETHI() PORTD |=  0x40
#define RESETLO() PORTD &= ~0x40

#define SPAREHI() PORTD |=  0x80
#define SPARELO() PORTD &= ~0x80

#define GETTICS() INTR_OFF(); tmptics=tics; INTR_ON()

//-------driver ins---------
#define MOLO()  (PINB & 0x02)==0 /* motor at zero */
//L0  L1
//------
// 0  0  thermal shutdown
// 0  1  overcurrent
// 1  0  open load
// 1  1  normal
#define LO0LO() (PINB & 0x04)==0 /* overtemp */
#define LO1LO() (PINB & 0x08)==0 /* overcurrent */

unsigned char MO;  //driver chip fault outputs
unsigned char LO0;
unsigned char LO1;

//servo params: lever span in deg, sweep time, lever pos in ad lsbs, deg, stps; deg per step,
//examples: PL1 60deg, .5sec, 

//----autothrottle conversion factors----
float kpotoneturn2deg = 1024.0/360.0;  //1024 lsbs in 360deg = 2.844 lsbs/deg   degs on bottom
float kpottenturn2deg = 1024.0/3600.0; //1024 lsbs in 3600deg = .2844 lsbs/deg 

float kpotlsb2volt = 5.0/1024.0;

float kpotlsb2pct = 100.0/1024.0;      //
float kpotlsb2deg = 360.0/1024.0;      //1024 lsbs in 360deg = .35156 lsbs/deg  lsbs on bottom  

float kepldeg2pct = 100.0/40.0;        //100pct is 40deg = 2.5   40deg throw on emer lever or tab = 100pct on bottom
float keplpct2deg = 40.0/100.0;        //40pct is 100deg = .4

float kpldeg2pct  = 100.0/60.0;        //60pct is 60deg  .6  60deg throw on power lever or tab = 100pct on bottom
float kplpct2deg  = 60.0/100.0;        //60deg is 100pct  1.66         

float kpropdeg2pct = 100.0/60.0;       //100pct is 60deg  .6  60deg throw on prop lever or tab = 100pct on bottom
float kproppct2deg = 60.0/100.0;       //60deg is 100pct  1.666

float kfueldeg2pct = 100.0/30.0;       //100pct is 30deg  .3   30deg throw on fuel lever or tab = 100pct on bottom
float kfuelpct2deg = 30.0/100.0;       //30deg is 100pct  3.333

float keplpct2stp  = 33.0/100.0;       //100pct is 33stps            100% on bottom
float kplpct2stp   = 100.0/33.0;       //

float kproppct2stp = 33.0/100.0;       //100pct is 33stps            100% on bottom
float kfuelpct2stp = 100.0/33.0;       //

float kepldeg2stp   = 22.2/40.0; //22.2 stps in 40 deg in 2 sec  90 ms/stp   deg on bottom
float kpldeg2stp    = 33.3/60.0; //33.3 stps in 60 deg in .5 sec 67 ms/stp
float kpropdeg2stp  = 33.0/60.0; //same
float kfuelpdeg2stp = 16.6/30.0; //16.6 steps in 30 deg in 2 sec 120 ms/stp
float kytrimdeg2stp; //5.5 turns of 200 steps=1100 stps x 1.8deg/stp = 1980 deg on 10 turn pot (3600deg) 29 sec.
float ketrimdeg2stp; //8 turns of 200 steps=1600 stps x 1.8deg/stp = 2880 deg out of 3600. 15ms/stp x 2880=43 sec.

float kmotdegperstp = 1.8;

//float kepllsb2stp =  33.0/40.0;  //stps
//float kpllsb2stp =   33.0/60.0;  //
//float kproplsb2stp = 33.0/60.0;  //
//float kfuellsb2stp = 33.0/30.0;  //

//float keplspan2deg = (40.0/360.0)*1023.0;
//float kplspan2deg  = (60.0/360.0)*1023.0;
//float kpropspan2deg = (60.0/360.0)*1023.0;
//float kfuelspan2deg = (30.0/360.0)*1023.0;

//-------servo vars---------------
float fpldeg;            //0-60  deg min to max of any lever. 3 different spans
float fpotdeg;           //0-60.0  
float fpotpct;           //0-100%
float fpotstp;           //0-33 steps in 60 deg
float fpotvlt;           //0-5v
float fsrvtabdeg;        //0-60 deg on stepper shaft<->same as tab on lever
float fsrvtabstp;        //60deg/1.8degperstep = 33 steps
float fsrvtabpct;        //0-100pct to send back to autopilot thru can

unsigned char pldempct;   //0-100pct demand in from autopilot over can
unsigned char pldempctl;  //pct last pass
unsigned char srvdemstps; //pl dem from ap to at
unsigned char srvposstps; //srv pos in stps
unsigned char potpct;     //0-100pct pl position out to autopilot over can
unsigned char srvtabpct;  //0-100 back to autopilot thru can
unsigned char latencystart,latencyend,latency; //req: must be < 50ms

unsigned int potraw;  //0-1023lsbs = 0-360deg  60deg out of 360 of tab to cable
unsigned int potavg;  //over 16 passes
unsigned int potavgl; //last pass
unsigned int potmin;  //pl pot value at 0 deg
unsigned int potmax;  //pl pot value at 60 deg
unsigned int potspan; //potmax-potmin= 60 deg 
unsigned int potzer;  //0 deg straight up
//unsigned int potm30;  //-30 deg 
//unsigned int potp30;  //+30 deg

unsigned int potdeg;    //trims can go to 3600 steps
unsigned int demstps;   //0-33 stps on pl calced from 100 pct demand from ap
unsigned int potstp;    //0-33 motor steps to move pl 60 deg

//#if 0
//--------------------------
void initservovars(void){
//read dipswitch to get servo type, init params for this type (span)
unsigned char n;
unsigned int potraw,potavg;

  n=32;
  while(n--){ //avg 100 readings
   	potraw = readadchan(0); //read servo position pot
	  potavg = calcrollavg(&rollavg[0],potraw); //avg over 30 samps
  }
  potspan = potmax - potmin; //units of lsbs
//  kpotlsb2deg = (potspan - potmin) * (240.0/360.0); //240 is lock to lock span, 360 deg ctl pot
  fpotdeg = potavg * kpotlsb2deg; //calc servo tab position in deg
	fpldeg = fpotdeg; //assume pl has same deg 
	cprintf("%5.1f fpldeg\n",fpldeg);
}
//#endif

//--------------------------
void potcal(void){
//use servo type from dipswitch to calc:
//show pot in raw,avg,volt,pct,deg,stp, save in eeprom?
unsigned char c;
unsigned int t1,t2,dt,dton,dtoff;

  cprintf("pot cal  p=disp\n");
	c=0;
  dtoff=0;
  potavg = 0;
  potmin = potmax = 0;
  potspan = 0;
	potzer = 0;
	fpotdeg = 0.0;
	
  cprintf("pot ccw to min, any key: ");
  c=getchar();
  potmin = readadchan(0); //read servo position pot
  cprintf("%4d min \n",potmin);
	
  cprintf("pot cw to max, any key: ");
  c=getchar();
  potmax = readadchan(0); //read servo position pot
  cprintf("%4d max \n",potmax);
	
  potspan = potmax - potmin;  //in units of pot lsbs
  cprintf("%4d span \n",potspan);
	
  cprintf("pot straight up to half, any key: ");
  c=getchar();
  potzer = readadchan(0); //read servo position pot
  cprintf("%4d half \n\n",potzer);
	
  cprintf("raw  avg  vlt   pct   deg   stp   dtn dtf\n");
	dispon=1;
  c=0;
  while(c != 'q'){
	  GETTICS();
	  t1=tmptics;
    if(kbhit()){
      c=getchar();
			if(c=='p') dispon = !dispon;
    }  
    potraw  = readadchan(0); //read servo position pot in lsbs
		potavg  = calcrollavg(&rollavg[0],potraw); //lsbs
    fpotvlt = potavg * kpotlsb2volt; //just for ref, not used in servo
    fpotpct = potavg * kpotlsb2pct;  //from pot in this routine, but from ap in aircraft
    fpotdeg = potavg * kpotlsb2deg; 
    fpotstp = fpotdeg * kpldeg2stp; 
    potstp  = (unsigned int)fpotstp;

  	if(dispon) cprintf("%4d %4d %#5.2f %#5.1f %#5.1f %4d %4d %4d\r",
      potraw,potavg,fpotvlt,fpotpct,fpotdeg,potstp,dton,dtoff);
		GETTICS();
		t2=tmptics;
		dt=t2-t1;
		if(dispon) dton=dt; else dtoff=dt;	
  }//while
  crlf();
}

//--------------------------
void stpcw(void){
//maybe take a step cw  2sec sweep =60ms .5sec sweep=15ms

//	if(srvdemstps > srvposstps){
  	ENABHI();
    STBYHI();
	  DIRCW();
    SPAREHI();
		STEPHI(); 
		NOP();
		STEPLO(); 
    SPARELO();
		srvposstps++;
		delnms(16);   //16ms is pl lever sweep 60 deg in .5sec 33steps
//  	ENABLO();
//    STBYLO();
//  }  
}

//--------------------------
void stpccw(void){
//	if(srvdemstps < srvposstps){
  	ENABHI();
    STBYHI();
	  DIRCCW();
    SPAREHI();
		STEPHI(); 
		NOP();
		STEPLO(); 
    SPARELO();
		srvposstps--;
		delnms(16); //pl sweeps in .5 sec, others diff
//  	ENABLO();
//    STBYLO();
//  }  
}

//--------------------------
void atservoloop(void){
//auto throttle servo loop
//recv CAN pct 0-100 = 0 - 60deg = 0 - 33steps
//read position pot 0-1023 = 0-165deg, calc stepdemand, 
//calc error, step stepper to null err every 60ms
//req: servo starts to move before 50ms after new can dem rcvd
unsigned char c;
unsigned int t1,t2,dt,dton,dtoff;

  initservovars();
  cprintf("servoloop azAZ=pct sx=enab dc=stby fv=dir spc=step gb=reset p=dispon  q=quit\n"
          "plpct fpldeg stpdem stppos potraw potavg fpotdeg potpct latency MO LO L1 dtn dtf\n");
	pldempct=pldempctl=0;
	dtoff=0;
	dispon = 1;			 
	c=0; 
	while(c != 'q'){
	  GETTICS(); //1ms tics
		t1=tmptics;
	  if(kbhit()){
		  c=getchar();
  		switch(c){
	  	  case 'a': if(pldempct < 100) pldempct++; break; //force pl demand position using keybrd
		    case 'z': if(pldempct > 0)   pldempct--; break;
		    case 'A': pldempct=100; break;
  		  case 'Z': pldempct=0;   break;
				case 's': ENABHI();  break;
				case 'x': ENABLO();  break;
				case 'd': STBYHI();  break;
				case 'c': STBYLO();  break;
				case 'f': DIRCW();   break;
				case 'v': DIRCCW();  break;
				case 'g': RESETHI(); break;
				case 'b': RESETLO(); break;
				case ' ': STEPHI(); NOP(); STEPLO(); break;
	  		case 'p': dispon = !dispon;
		  }//switch
		}//kbhit
		//pldempct simulates receiving can pkt from flight control computer 
    //todo: read can, read plpct from can  or use plpct from keybd
		if(pldempct != pldempctl){ //keystroke simulates can pkt arrival
		  GETTICS();
		  latencystart=tmptics;
		}
		pldempctl = pldempct;	//remember pldempct
		
		//calc servo dem in stps fn of plpct
    fpldeg   = pldempct*kplpct2deg;       //60 deg = 100 pct
		fsrvtabdeg = fpldeg;                 //0-60 deg. same angle as pl lever
		fsrvtabstp = fsrvtabdeg/kmotdegperstp;  //60deg/1.8degperstep 
		srvdemstps = (unsigned int)fsrvtabstp; //float to int		

    //take a step iff ne
		if(srvdemstps > srvposstps){
  		stpcw();                   //this should take a step cw
	  }
		if(srvdemstps < srvposstps){
		  stpccw();
    }			
     
		//read position pot.   
    potraw = readadchan(0); //0-1023lsbs 0-240deg
		potavg = calcrollavg(&rollavg[0],potraw);
		if(potavg != potavgl){
		  GETTICS(); 
			latencyend = tmptics;
		}
		potavgl = potavg; //remember last pass
		if(latencystart != latencyend) latency = (latencyend - latencystart)-60; //latency in ms
		
    fpotdeg = potavg * kpotlsb2deg; //1023 lsbs/330 deg= 3.1 lsbs/deg  using 1 turn pot ; 
		potdeg = (unsigned int)fpotdeg;
//    fpotdeg = potavg*kpottenturn2deg; //1023 lsbs/3600 deg= .28 lsbs/deg using 10 turn pot on trims

    fsrvtabpct = fpotdeg*kpldeg2pct;   //maybe use srvtab here (position of motor)
		potpct = (unsigned char)fsrvtabpct; //float to int
		 
		//send srvtabpct back to autopilot over can here boss

		MO  = MOLO();  //status outs from chip
		LO0 = LO0LO(); //overtemp
		LO1 = LO1LO(); //overcurrent
		
	  if(dispon){
      cprintf("%3d    %#5.1f  %4d   %4d   %4d   %4d   %#5.1f  %4d   %4d    %1d  %1d  %1d  %3d %3d\r",
		  pldempct,fpldeg,srvdemstps,srvposstps,potraw,potavg,fpotdeg,potpct,latency,MO,LO0,LO1,dton,dtoff);
    }  
		GETTICS(); //1ms tics	
	  t2=tmptics;
	  dt=(t2-t1);
		if(dispon) dton=dt;
		if(!dispon) dtoff=dt;
	}//while
//	crlf();	
}

//--------------------------
void plmax(void){
//drive power lever to max from keyboard
//pl to max 60 deg= 33 steps (1.8deg/step) in .5 sec (66 steps sec, 16ms per step)

  cprintf("");
  srvposstps = 0;
	srvdemstps = 33;  //33steps x 1.8 deg/step = 60 deg	 .5sec-> 
	ENABHI();
	STBYHI();
	while(srvdemstps != srvposstps){
		if(srvdemstps > srvposstps){
  		stpcw();
	  }
		if(srvdemstps < srvposstps){
		  stpccw();
    }			
    cprintf("%d %d \r",srvdemstps,srvposstps);
	}//while		
}

//--------------------------
void plmin(void){
//pl to max 60 deg= 32 steps (1.8deg/step) in 2 sec (16 steps sec, 62ms per step)

  srvposstps=33;
	srvdemstps = 0;  //		
	ENABHI();
	STBYHI();
	while(srvdemstps != srvposstps){
		if(srvdemstps > srvposstps){
  		DIRCW();  STEPHI(); delnms(16); STEPLO(); srvposstps++;
	  }
		if(srvdemstps < srvposstps){
  		DIRCW();  STEPHI(); delnms(16); STEPLO(); srvposstps--;
    }			
    cprintf("%d %d \r",srvdemstps,srvposstps);
	}//while		
}

//------------------------
void stpmenu(void){
  cprintf("az=dir sx=enab dc=stby spc=stp \n");
}
    
//------------------------
void stepopenloop(void){
//step up or down 
unsigned char c;

  cprintf("Step open loop\n");
	c=0;
	stpmenu();
	ENABLO();
	STBYLO();
	while(c != 'q'){
	  if(kbhit()){
		  c=getchar();
			switch(c){
			  case 'a': DIRCW(); break; 
			  case 'z': DIRCCW(); break;
				case 's': ENABHI(); break;
				case 'x': ENABLO(); break;
				case 'd': STBYHI(); break;
				case 'c': STBYLO(); break;
				case ' ': STEPHI(); break;
			}//switch
		}//if kbhit	
    cprintf("%4u \r",srvposstps);
	}//while
}

//--------------------------
void atservomenu(void){
//cmds 

  cprintf("ATservo cmds:\n"
          " 1 readadloop\n"
          " 2 stepopenloop\n"
          " 3 atservoloop\n"
          " 4 potcal\n"
          " 5 plmax\n"
          " 6 plmin\n"
          " 7 enabhi\n"
          " 8 enablo\n"
          " 9 stbyhi\n"
          " 0 stbylo\n"
          " d debugloop\n"
      );
}
      
//-----------------
void main(void){
//ATservoTB67S128UNO main program
unsigned char c,n;

  init_devices(); //turns on interrupts
	initrollavg(&rollavg[0]);
  initscreen();   //banner on serial
	initservovars();
	del=50; //1ms tics
	c=0;
  while(1){      //calibrate and adjust loop
    atservomenu();
    crlf();
    putc('>');   //prompt for input
    c=getche();  //get the char and echo it
    crlf();
//		LEDON();
    switch(c){
      case '1': readadloop();  break;
      case '2': stepopenloop(); break;
      case '3': atservoloop(); break;
      case '4': potcal();      break;
      case '5': plmax();       break;
      case '6': plmin();       break;
      case '7': ENABHI();      break;
      case '8': ENABLO();      break;
      case '9': SPAREHI();     break;
      case '0': SPARELO();     break;
      case 'd': debugloop();   break;
//      default: mainmenu();
    }
		_StackCheck(); //during debug
  }//while
}

//---strings embedded in rom--------------
__flash char copyrightstring[]={"ATservoTB67S128UNO by Bob Gardner at aol.com June 6 2022"};
//--------eof-------------