void InitApp(void)
{
/*PORT*/
TRISA = 0x0000;
TRISB = 0x0F80;
LATA = 0;
LATB = 0;
CNPUBbits.CNPUB7 = 1;
CNPUBbits.CNPUB8 = 1;
/* Setup analog functionality and port direction */
ANSELA = 0;
ANSELB = 0;
/* Initialize peripherals */
InitQEI();
InitPWM();
InitI2C();
I2C2ADD = 20;
// InitT2T3();
// InitT1();
InitT2();
POS1CNTH = 0x4000; //32bit mode
nRESOLUTION = 26880; //4*64*105
nTARGET = 0x40000000;
}
int main() {
unsigned char Lcd_LINE1[6] = {'\0'};
unsigned char Lcd_LINE2[16] = {'\0'};
unsigned int i;
InitCan();
InitQEI();
InitPwm();
// InitInt();
InitAdc();
// InitUart();
//pid
unsigned int *pwmOUT;
pid_t mypid;
float degPOT = 0.0;
float degMTR = 0.0;
while (1) {
if(InData0[1] > 0){
// if(InData0[2] == 1){
// PDC2 = (unsigned int)(InData0[1]* 2 *.5914);
// PDC1 = 0;
// }
// else{
// PDC1 = (unsigned int)(InData0[1]* 2* .5914);
// PDC2 = 0;
// }
pwmOUT = CalcPid(&mypid, degPOT, degMTR);
PDC1 = *(pwmOUT + 0); // 16-bit register
PDC2 = *(pwmOUT + 1); // 16-bit register
}
if (InData0[3] == 1) {
C1TX0B4 = 2;
C1TX0B1 = POSCNT;
C1TX0B2 = C1RX0B2;
C1TX0B3 = C1RX0B3;
C1TX0CONbits.TXREQ = 1;
while (C1TX0CONbits.TXREQ != 0);
}
msDelay(10);
} //while
} // main
int main() {
unsigned char Lcd_LINE1[6] = {'\0'};
unsigned char Lcd_LINE2[16] = {'\0'};
unsigned int i;
InitCan();
InitQEI();
InitPwm();
// InitInt();
InitAdc();
// InitUart();
while (1) {
if(InData0[1] > 0){
if(InData0[2] == 1){
PDC2 = (unsigned int)(InData0[1]* 2 *.5914);
PDC1 = 0;
}
else{
PDC1 = (unsigned int)(InData0[1]* 2* .5914);
PDC2 = 0;
}
}
if (InData0[3] == 1) {
C1TX0B4 = 2;
C1TX0B1 = POSCNT;
C1TX0B2 = C1RX0B2;
C1TX0B3 = C1RX0B3;
C1TX0CONbits.TXREQ = 1;
while (C1TX0CONbits.TXREQ != 0);
}
msDelay(10);
} //while
} // main
/******************************************************************
Main
******************************************************************/
int main(void)
{
/*Disable watchdog*/
RCONbits.SWDTEN = 0;
set_clk();
InitIO();
//Initialize CAN communication
initialisation_CAN();
//Initialize PWM (Motor Control)
Init_PWM();
//Initialize ADC
//Init_ADC();
//Initialize QEI (Speed/Position DC motor)
InitQEI();
chinookpack_unpacked_init(&unpacker);
#ifdef memory_init
/*Retrieve Last gear and mât position*/
do{
EEPROM_REQUEST = 0x55;
envoi_CAN_periodique_init();
last_gear = datReceive_can_EEPROM_CONFIG_ANSWER_gear;
last_position_mat = datReceive_can_EEPROM_CONFIG_ANSWER_mat;
}while(datReceive_can_EEPROM_CONFIG_ANSWER_mat==190.0f);
gear = last_gear;
Position_mat = last_position_mat;
char print[80];
sprintf(print,"last gear: %d \t last position mat : %f \r",last_gear,last_position_mat);
char u=0;
do
{
U1TXREG=print[u];
while(U1STAbits.TRMT!=1);
u++;
}while(print[u]!=0);
int k=0;
do{
EEPROM_REQUEST = 0xAA;
envoi_CAN_periodique_init();
k++;
}while(k<5);
#endif
/*Enable 24V supply switch*/
ENALIM = 1;
//envoi_CAN_periodique();
//envoi_CAN_periodique();
//Transmission goes to first gear
Stepper_Shift_Init(last_gear);
//Mat goes to origin
//Init_mat(last_position_mat);
while(1)
{
//Transmission goes to first gear
if(datReceive_can_conf)
Stepper_Shift_Init(last_gear);
Stepper_Shift();
envoi_CAN_periodique();
#ifdef mat_manuel
if(!datReceive_can_cmd[0])
{
if(last_cmd_mat == 14 || last_cmd_mat == 15){
cmd_mat=datReceive_can_cmd[3]|datReceive_can_cmd[2]|datReceive_can_cmd[1]|datReceive_can_cmd[0];
}
if(last_cmd_mat == 11){
delai_mat++;
cmd_mat = 0;
if(delai_mat<60000)
tower_motor_ctrl(&PDC1,Position_mat);
else if(delai_mat>=60000)
delai_mat = 0;
}
}
else if(!datReceive_can_cmd[2])
{
if(last_cmd_mat == 11 || last_cmd_mat == 15){
cmd_mat=datReceive_can_cmd[3]|datReceive_can_cmd[2]|datReceive_can_cmd[1]|datReceive_can_cmd[0];
}
if(last_cmd_mat == 14){
cmd_mat = 0;
delai_mat++;
if(delai_mat<60000)
tower_motor_ctrl(&PDC1,Position_mat);
else if(delai_mat>=60000)
delai_mat = 0;
}
}
else //if(datReceive_can_cmd == 15)
{
cmd_mat=15;
}
if(cmd_mat ==14)
{
last_cmd_mat = cmd_mat;
LED0^=1;
tower_motor_ctrl(&PDC1,-180.0f);
}
else if(cmd_mat == 11)
{
last_cmd_mat = cmd_mat;
tower_motor_ctrl(&PDC1,180.0f);
}
else if(cmd_mat == 15)
{
last_cmd_mat = cmd_mat;
tower_motor_ctrl(&PDC1,Position_mat);
}
#endif
#ifdef mat_test
tower_motor_ctrl(&PDC1,-90.0f);
#endif
#ifdef mat_auto
tower_motor_ctrl(&PDC1,datReceive_can_wind_direction);
#endif
}
}
int main() {
unsigned int i;
InitCan();
InitQEI();
InitPwm();
// InitAdc();
pid_t mypid;
TRISRED = 0; // PORTE output
TRISYLW = 0; // PORTE output
TRISGRN = 0;
// Misc. variables
float degPOT = 0.0;
float degMTR = 0.0;
unsigned int *pwmOUT;
while (1) {
// Motor PWM control state machine
switch (motorState) {
case INITIALIZE:
LEDRED = 1;
LEDYLW = 0;
LEDGRN = 0;
// Initialization to offset POSCNT to three turns (12000 counts)
POSCNT = 12000; // This prevents under and overflow of the POSCNT register
// Enable ADC Module
ADCON1bits.ADON = 1; // A/D converter module on
// Enable PWM Module
PTCONbits.PTEN = 1;
// Initialize PID
InitPid(&mypid, PID_KP, PID_KD, PID_KI, PID_TS, PID_N, 0, 0, 0, 0);
// Enable CAN module
C1CTRLbits.REQOP = NORMAL;
while (C1CTRLbits.OPMODE != NORMAL);
// motorState = SEND_DATA;
break;
// Send data packet
case SEND_DATA:
LEDRED = 0;
LEDYLW ^= 1;
LEDGRN = 0;
C1TX0B4 = PIC_ID;
C1TX0B1 = POSCNT;
C1TX0B2 = (unsigned int) ((InData0[1] * 2*PWM_COUNTS_PERIOD) / HAPTIC_RANGE);;
C1TX0B3 = C1RX0B3;
C1TX0CONbits.TXREQ = 1;
while (C1TX0CONbits.TXREQ != 0);
break;
case SEND_HOME:
// Homing sequence
LEDRED = 0;
LEDYLW = 1;
LEDGRN = 0;
motorState = SEND_HOME;
break;
default :
// haptic code
LEDRED = 0;
LEDYLW = 0;
LEDGRN = 0;
// if (InData0[1] > 0) {
// if (InData0[2] == 1) {
// PDC2 = (unsigned int) ((InData0[1] * 2*PWM_COUNTS_PERIOD) / HAPTIC_RANGE);
// PDC1 = 0;
// } else {
// PDC1 = (unsigned int) ((InData0[1] * 2*PWM_COUNTS_PERIOD)/HAPTIC_RANGE);
// PDC2 = 0;
// }
// }
break;
} // motorState switch
// PDC1 = *(pwmOUT + 0); // 16-bit register
// PDC2 = *(pwmOUT + 1); // 16-bit register
msDelay(PID_TS);
} //while
} // main
