*      Serial Servo Controller RobotHand
*/

#include "pic.h"
#include "delay.c"
static bit Servo1       @ ((unsigned)&PORTB*8+0);       // R/W
static bit Servo2       @ ((unsigned)&PORTB*8+1);       // R/W
static bit Servo3       @ ((unsigned)&PORTB*8+2);       // R/W
static bit Servo4       @ ((unsigned)&PORTB*8+3);       // R/W
static bit Servo5       @ ((unsigned)&PORTB*8+4);       // R/W
static bit Servo6       @ ((unsigned)&PORTB*8+5);       // R/W
static bit Servo7       @ ((unsigned)&PORTB*8+6);       // R/W
static bit Servo8       @ ((unsigned)&PORTB*8+7);       // R/W

extern void Var_Init(void);  //prototipi        
extern void Agg_Pos(void);
extern void Par_ser(void);
unsigned char S1H,S2H,S3H,S4H,S5H,S6H,S7H,S8H;  //variabili
unsigned char S1L,S2L,S3L,S4L,S5L,S6L,S7L,S8L;
unsigned char C1,C2,C3,C4,RxPos,Flags;
int Pos1,Pos2,Pos3,Pos4,Pos5,Pos6,Pos7,Pos8;
 
main(void)
{
PORTA = 0x00;           // azzera le porte
PORTB = 0x00;   
PORTC = 0x00;
 
TRISA = 0x00;           // tutte uscite per evitare interferenze
TRISB = 0x00;           // uscite per servi
TRISC = 0xf8;           // bit 6-7 Seriale
 
Var_Init();             // init variabili posizione
ADCON1= 0x87;           // PORTA all digital mode
 
//Inizializzazione uart
TXSTA= 0x20;            // TX abilitato
RCSTA= 0X90;            // Registro RX USART
BRGH = 1;               // USART High velocity
SPBRG= 25;              // Baud Rate Generator (25=9600 baud)
 
//Inizializzazione TIMER1
T1CON   = 0;            // TIMER 1 resettato
T1CKPS1 = 0,T1CKPS0=0;  // TIMER1 prescaler = 1
T1OSCEN = 0;            // TIMER1 external oscillator disable
TMR1CS  = 0;            // TIMER1 internal clock (fosc/4)
TMR1ON  = 0;            // TIMER1 in STOP
 
//Inizializzazione interrupt
INTCON = 0;             // Interrupt disabilitato
PIE1   = 0;             // Interrupt di periferica disabilitati
GIE    = 1;             // Global Interrupt Enable
PEIE   = 1;             // Periferal Interrupt Enable
RCIE   = 1;             // USART Receive Interrupt Enable
 
for(;;)
 {
 Servo1 = 1;           // Servo out High
 TMR1H=S1H;             // Durata impulso
 TMR1L=S1L;     
 TMR1ON = 1;            // TIMER1 ON
  while (TMR1IF==0){}   // Controllo flag interrupt timer 0
   Servo1 = 0;          // Servo out Low
   TMR1IF = 0;          // Ripristino flag Interrupt
   TMR1ON = 0;          // TIMER1 STOP
 
 Servo2 = 1;
 TMR1H=S2H;
 TMR1L=S2L;     
 TMR1ON = 1;
  while (TMR1IF==0){}
   Servo2 = 0;
   TMR1IF = 0;
   TMR1ON = 0;
 
 Servo3 = 1;
 TMR1H=S3H;
 TMR1L=S3L;     
 TMR1ON = 1;
  while (TMR1IF==0){}
   Servo3 = 0;
   TMR1IF = 0;
   TMR1ON = 0;
 
 Servo4 = 1;
 TMR1H=S4H;
 TMR1L=S4L;     
 TMR1ON = 1;
  while (TMR1IF==0){}
   Servo4 = 0;
   TMR1IF = 0;
   TMR1ON = 0;
 
 Servo5 = 1;
 TMR1H=S5H;
 TMR1L=S5L;     
 TMR1ON = 1;
  while (TMR1IF==0){}
   Servo5 = 0;
   TMR1IF = 0;
   TMR1ON = 0;
 
 Servo6 = 1;
 TMR1H=S6H;
 TMR1L=S6L;     
 TMR1ON = 1;
  while (TMR1IF==0){}
   Servo6 = 0;
   TMR1IF = 0;
   TMR1ON = 0;
 
 Servo7 = 1;
 TMR1H=S7H;
 TMR1L=S7L;     
 TMR1ON = 1;
  while (TMR1IF==0){}
   Servo7 = 0;
   TMR1IF = 0;
   TMR1ON = 0;
 
 Servo8 = 1;
 TMR1H=S8H;
 TMR1L=S8L;     
 TMR1ON = 1;
  while (TMR1IF==0){}
   Servo8 = 0;
   TMR1IF = 0;
   TMR1ON = 0;
 
 if (Flags==1) Par_ser();
   
 Agg_Pos();                     // Sposta Servi
 DelayMs(2);                    // Delay aggiuntivo
 }
}
 
void
Var_Init(void)
{
S1H= 0xFF;              // Servo neutral (1.5ms)
S1L= 0X00;
S2H= 0xFA;              
S2L= 0X23;
S3H= 0xFA;
S3L= 0X23;
S4H= 0xFA;
S4L= 0X23;
S5H= 0xFA;
S5L= 0X23;
S6H= 0xFA;
S6L= 0X23;
S7H= 0xFA;
S7L= 0X23;
S8H= 0xFA;
S8L= 0X23;
 
Pos1 = 500;             // Nuova posizione (1.5 ms)
Pos2 = 500;
Pos3 = 500;
Pos4 = 500;
Pos5 = 500;
Pos6 = 500;
Pos7 = 500;
Pos8 = 500;
 
RxPos = 0;
Flags  = 0;
}
 
void
Agg_Pos(void)
{
S1H = (64535-Pos1)/256;
S1L = (64535-Pos1)-S1H*256;
 
S2H = (64535-Pos2)/256;
S2L = (64535-Pos2)-S2H*256;
 
S3H = (64535-Pos3)/256;
S3L = (64535-Pos3)-S3H*256;
 
S4H = (64535-Pos4)/256;
S4L = (64535-Pos4)-S4H*256;
 
S5H = (64535-Pos5)/256;
S5L = (64535-Pos5)-S5H*256;
 
S6H = (64535-Pos6)/256;
S6L = (64535-Pos6)-S6H*256;
 
S7H = (64535-Pos7)/256;
S7L = (64535-Pos7)-S7H*256;
 
S8H = (64535-Pos8)/256;
S8L = (64535-Pos8)-S8H*256;
}
 
void interrupt SerChar(void)  //prototipo ricezione
{
  if (RCREG==0x40 | RxPos > 6)  //se arriva la chiocciola dimensiona la stringa
   {
   C1=0,C2=0,C3=0,C4=0;         //azzero la stringa
   RxPos=0,Flags=0;
   }
  if (RxPos==1) C1=RCREG;       //ricezione della stringa comandi
  if (RxPos==2) C2=RCREG; 
  if (RxPos==3) C3=RCREG;
  if (RxPos==4) C4=RCREG;
  if (RxPos==5 & RCREG==0x0d) Flags=1; // <CR> terminator
  RxPos++;
}
 
void
Par_ser(void)           // Analisi comandi
{
 Flags=0;
 switch (C1)
  {
  case 0x41:    //carattere A
   Pos1 = C2*4; //Pos1 assume il valore C2 trasmesso con A
   break;
  case 0x42:    //carattere B
   Pos2 = C2*4;
   break;
  case 0x43:    //carattere C
   Pos3 = C2*4;
   break;
  case 0x44:    //carattere D
   Pos4 = C2*4;
   break;
  case 0x45:    //carattere E
   Pos5 = C2*4;
   break;
  case 0x46:    //carattere F
   Pos6 = C2*4;
   break;
  case 0x47:    //carattere G
   Pos7 = C2*4;
   break;
  case 0x48:    //carattere H
   Pos8 = C2*4;
   break;
  }
}