//-----------------------------------------------------------------------------
// Servo control with dsPIC 30F4012
// L. BAGHLI 30/10/2015
//-----------------------------------------------------------------------------
#include "p30F4012.h"

//Configuration bits
/// pas de Quartz !
// _FOSC(CSW_FSCM_OFF & FRC_PLL8);	// 7.37Mhz *8 = 58.96 /4 = 14.74 MIPS maxi pour ce pic
_FOSC(CSW_FSCM_OFF & FRC);	// 7.37Mhz /4 = 1.8425 MIPS pour ce pic
_FWDT(WDT_OFF);
_FBORPOR(PBOR_OFF & RST_PWMPIN & PWMxH_ACT_HI & PWMxL_ACT_LO &  PWRT_64 & MCLR_EN);
//_FGS(CODE_PROT_OFF);
//-----------------------------------------------------------------------------
//Program Specific Constants
//#define FCY 14740000					//Instruction cycle rate (Osc x PLL / 4) = 14.74 MIPS
#define FCY 1842500					//Instruction cycle rate (Osc / 4 ) = 1.8425 MIPS 
#define MILLISEC FCY/1842			// 1 mSec delay constant

// Pour FRC_PLL8    FCY 14740000
////#define FullDuty 18425				// 14.74 MHz*20 ms =294800 / 16 (prescaler 1:16) = 18425
//#define FullDuty 16583				// 18 ms 
//#define Duty100  3869               // 2.1 ms --> 1934.625 x2
//#define Duty50   2764               // 1.5 ms --> 1381.875 x2
//#define Duty0    1658                // 0.9 ms --> 829.125 x2
//#define coefadc 2.1591796875     // rapport (Duty100-Duty0) / 1024

// Pour FRC     FCY 1842500
#define FullDuty 9212				// 1842500 Hz*20 ms =294800 / 4 (prescaler 1:4) = 9212.5
//#define FullDuty 16583				// 18 ms 
#define Duty100  1935               // 2.1 ms --> 967.3125 x2 = 1934.625 resolution PWM double
#define Duty50   1382               // 1.5 ms --> x2
#define Duty0    829.125                // 0.9 ms --> x2 829.125
#define coefadc 1.07958984375     // rapport (Duty100-Duty0) / 1024
// AN0 entrée Potar
// PWM1H sortie servo

void setup_ports(void);
void InitADC10();
void InitMCPWM();
void DelayNmSec(unsigned int N);
void Delay(unsigned int N);

int ValPot;

//-----------------------------------------------------------------------------
//	Setup 
//-----------------------------------------------------------------------------
void setup_ports(void)
{
	ADPCFG=0xFFFE;	// AN0 entrée Potar, all others PORTB = Digital(1) 
	// Clear All Ports Prior to defining I/O
	PORTB=0;	//Initialize LED pin data to off state
	PORTC=0;
	PORTD=0;
	PORTE=0;	
	// Now set pin direction registers
	TRISB = 0xFFFF;		// entrées
	TRISC = 0xFFFF;		// entrées
	TRISD = 0xFFFF;		// entrées
	TRISE = 0xFFFD;		// PWM1H sortie : 1101
	TRISF = 0xFFFF;		// entrées
}
//-----------------------------------------------------------------------------
void InitADC10()
{
    ADCON1 = 0x006F;				// ADC Sampling auto , SOC by PWM
    ADCON2 = 0x0000;				// CH0 conversion only
    ADCHS  = 0x0000;				// Channel 0 positive input is AN0 et negative input is VREF-
    ADCON3 = 0x0080;				// Tad = internal RC (4uS)
    _ADIF = 0;      // Adc int flag Off
    _ADIE = 1;      // Adc int On
	_ADON = 1;		// turn ADC ON
}
//-----------------------------------------------------------------------------
//  InitMCPWM, intializes the PWM as follows:
//  FPWM = 16 khz voir en haut
//  Independant PWMs
//  Set ADC to be triggered by PWM special trigger
//-----------------------------------------------------------------------------
void InitMCPWM()
{
	PTPER = FullDuty;		// TPWM = 20 ms
	OVDCON = 0xFFFF;	// Cmde MLI, no effect of OVDCON
	PWMCON1= 0x0110;    // enable PWM outputs PWM1H only independant
	PDC1=Duty50;		// init sans rien, apres une regul ça change
	SEVTCMP = FullDuty;		// set ADC to trigeer at ...
	PWMCON2 = 0x0000;		// 1 PWM values
	//PTCON = 0x8008;			// start PWM asymetrique prescale 1:16
	PTCON = 0x8004;			// start PWM asymetrique prescale 1:4
}
//?????????????????????????????????????????????????????????????????????
// The ADC interrupt reads the demand pot value.
// tt est synchrone % à cette int de ADC int =2*PWMperiod=2*62.5 us=125 us
//?????????????????????????????????????????????????????????????????????
void __attribute__((interrupt, auto_psv)) _ADCInterrupt ()
{
    _ADIF = 0;
    ValPot = coefadc*ADCBUF0; // ADC read, potentiometer value
    PDC1 = Duty0+ValPot;
}
//-----------------------------------------------------------------------------
//Main routine
int main(void)
{
	setup_ports();
    InitADC10();
    InitMCPWM();
	while(1)
		{	
		}	// end of while (1)
}

//=============================================================================
//Error traps
//-----------------------------------------------------------------------------
//Oscillator Fail Error trap routine
void __attribute__((interrupt, auto_psv)) _OscillatorFail(void)
{	
	while(1); //Wait forever
}
//-----------------------------------------------------------------------------
//Address Error trap routine
void __attribute__((interrupt, auto_psv)) _AddressError(void)
{
	while(1); //Wait forever
}
//-----------------------------------------------------------------------------
//Stack Error trap routine
void __attribute__((interrupt, auto_psv)) _StackError(void)
{
	while(1); //Wait forever
}
//-----------------------------------------------------------------------------
//Math (Arithmetic) Error trap routine
void __attribute__((interrupt, auto_psv)) _MathError(void)
{
    while(1); //Wait forever
}
//---------------------------------------------------------------------
// This is a generic 1ms delay routine to give a 1mS to 65.5 Seconds delay
// For N = 1 the delay is 1 mS, for N = 65535 the delay is 65,535 mS. 
// Note that FCY is used in the computation.  Please make the necessary
// Changes(PLLx4 or PLLx8 etc) to compute the right FCY as in the define
// statement above.
//---------------------------------------------------------------------
void DelayNmSec(unsigned int N)
{
unsigned int j;
while(N--)
 	for(j=0;j < MILLISEC;j++);
}
//---------------------------------------------------------------------
void Delay(unsigned int N)
{
unsigned int j;
 	for(j=0;j < N; j++) __builtin_nop();
}
//---------------------------------------------------------------------