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
void main(void) {
int t=15;
/* include your code here */
f1.functionId=ULTRASONIC_FRONT;
f1.status=READY;
f1.root=UNIQUE_FUNCTION;
f1.timerCount=50;
DisableInterrupts;
InitReg();
InitClock();
InitComunication();
InitPorts();
InitInputCompare();
InitKbi();
beginComunication();
initExecutingVector();
InitBuffer(&bufferIn);
InitBuffer(&bufferOut);
InitPwm();
InitADC();
EnableInterrupts;
InitRtc();
/*
despues de habilitar interrupciones y
antes de empezar a mover el robot debe calibrar sensores de meta
no arrancar el motor sin antes ejecutar initGoalSensor()!
*/
initGoalSensor();
setPwmValue(t);
if(t>35){
kbiSampleFreq=1000;
}else{
kbiSampleFreq=1000+((35-t)*200);
}
for(;;) {
if(goalStatus == 0){
PTDD_PTDD1;
setGoalMode(STOP_ON_GOAL);
SENTIDO_M1_1=1;
SENTIDO_M1_2=0;
SENTIDO_M2_1=1;
SENTIDO_M2_2=0;
SENSOR_DE_META_ON;
PTDD_PTDD0=1;
/*
functionHandler();
dispatcher(&executingVector,&bufferOut);
frameGenerator();*/
/* f1.functionId=ULTRASONIC_FRONT;
f1.status=READY;
getUltrasonic(&f1);
while(f1.status!=AVAILABLE);
t=3000;
while(t!=0){
t--;
} */
/*
f1.functionId=ULTRASONIC_FRONT;
f1.status=READY;
f1.root=UNIQUE_FUNCTION;
f1.timerCount=50;
getUltrasonic(&f1);
while(f1.status!=AVAILABLE);
t=3000;
while(t!=0){
t--;
} */
/*
f1.functionId=ULTRASONIC_RIGHT;
f1.status=READY;
getUltrasonic(&f1);
while(f1.status!=AVAILABLE);
t=30000;
while(t!=0){
t--;
} */
} /* loop forever */
}
/* please make sure that you never leave main */
}
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
