void setup(void) {
//----------- Setup ----------------
fruitInit();
pinModeDigitalOut(LED); // set the LED pin mode to digital out
digitalClear(LED); // clear the LED
delayStart(mainDelay, 5000); // init the mainDelay to 5 ms
delayStart(secondesDelay, 1000000); // init the secondesDelay to 1 s
//----------- Analog setup ----------------
analogInit(); // init analog module
analogSelect(0,MOTA_CURRENT); // assign MotorA current sense to analog channel 0
analogSelect(1,MOTB_CURRENT); // assign MotorA current sense to analog channel 0
//----------- dcmotor setup ----------------
//DCMOTOR_INIT(A);
//DCMOTOR_INIT(B);
dcmotorInit(A);
dcmotorInit(B);
rampInit(&rampeVitesseA);
rampInit(&rampeVitesseB);
//--- on/off switch
pinModeDigitalIn(ONOFF); // set the ONOFF pin mode to digital input
EEreadMain();
}
void loop() {
// ---------- Main loop ------------
fraiseService(); // listen to Fraise events
analogService(); // analog management routine
if(delayFinished(mainDelay)) // when mainDelay triggers :
{
delayStart(mainDelay, 5000); // re-init mainDelay
analogSend(); // send analog channels that changed
rampCompute(&rampeVitesseA);
rampCompute(&rampeVitesseB);
DCMOTOR(A).Vars.PWMConsign = rampGetPos(&rampeVitesseA);
DCMOTOR(B).Vars.PWMConsign = rampGetPos(&rampeVitesseB);
if(analogGet(0) > COURANT_MAX) { if(countA < COUNT_MAX) countA++; }
else { if(countA > 0) countA--; }
if(analogGet(1) > COURANT_MAX) { if(countB < COUNT_MAX) countB++; }
else { if(countB > 0) countB--; }
if(countA == 0) { digitalSet(MAEN); digitalSet(MAEN2); }
else if(countA == COUNT_MAX) { digitalClear(MAEN); digitalClear(MAEN2); }
if(countB == 0) { digitalSet(MBEN); digitalSet(MBEN2); }
else if(countB == COUNT_MAX) { digitalClear(MBEN); digitalClear(MBEN2); }
if(digitalRead(ONOFF)) {
rampGoto(&rampeVitesseA, 0) ;
rampGoto(&rampeVitesseB, 0) ;
timeReset();
}
DCMOTOR_COMPUTE(A,SYM);
DCMOTOR_COMPUTE(B,SYM);
if(i++ == 10) {
i = 0;
printf("C P %d %d %d\n",DCMOTOR(A).Vars.PWMConsign, DCMOTOR(B).Vars.PWMConsign, digitalRead(ONOFF));
}
}
if(delayFinished(secondesDelay)) // when secondesDelay triggers :
{
delayStart(secondesDelay, 1000000); // init the secondesDelay to 1 s
Seconds++;
if(++secondes >= 60) {
secondes = 0;
if(++minutes >= 60) {
minutes = 0;
heures++;
}
}
sequenceCompute();
}
}
void MainWindow::pressedStartStop()
{
if (started)
stop();
else if (ui->checkBoxTimed->isChecked())
delayStart(); // Reduce effect of tapping "Start" on the readings
else
start();
}
void setup(void)
{
//unsigned long t3,servoSOF;
fruitInit();
switchInit();
//i2cm_init(I2C_MASTER, I2C_SLEW_ON, FOSC/400000/4-1) ; //400kHz : clock = FOSC/(4 * (SSPADD + 1)) : SSPADD=FOSC/clock/4-1
//HMC5883_init();
// --------- init motors PWM on TMR2 ----------- // now done by core & dcmotor
// PWM on Timer2 : fPWM = fOSC / (TMR2prescale * 4 * (PR2+1))
// = 64000000Hz/(4 * 4 * 256) = 15625Hz
/*CCP2CON=CCP3CON=0b00001100 ; // single PWM active high
T2CONbits.T2CKPS0=1; //prescaler 4
PR2=255;
T2CONbits.TMR2ON=1;*/
// ------- init PWM1 to pulse MOTC_IN1
//PSTR1CON=0;
//PSTR1CONbits.STR1D=1;
// ------- init PWM2 to pulse MOTD_EN
//PSTR2CON=0;
//PSTR2CONbits.STR2B=1;
#if 1
// ---------- capture timer : TMR1 ------------
T1CON=0b00110011;//src=fosc/4,ps=8,16bit r/w,on.
PIE1bits.TMR1IE=1; //1;
IPR1bits.TMR1IP=1;
// ---------- Servos ------------
//Servo_Init();
//SERVO_SET_PORT(0,K3);
dcmotorInit(X);
DCMOTOR(X).Setting.PosWindow = 1;
DCMOTOR(X).Setting.PwmMin = 50;
DCMOTOR(X).Setting.PosErrorGain = 6;
DCMOTOR(X).Setting.onlyPositive = 0;
DCMOTOR(X).PosRamp.maxSpeed = 40;
DCMOTOR(X).PosRamp.maxAccel = 50;
DCMOTOR(X).PosRamp.maxDecel = 50;
rampSetPos(&DCMOTOR(X).PosRamp, 0);
DCMOTOR(X).PosPID.GainP = 120; //90
DCMOTOR(X).PosPID.GainI = 10;
DCMOTOR(X).PosPID.GainD = 0;
DCMOTOR(X).PosPID.MaxOut = 1023;
DCMOTOR(X).VolVars.homed = 1;
pinModeDigitalOut(MXEN2);
digitalSet(MXEN2);
dcmotorInit(Y);
DCMOTOR(Y).Setting.PosWindow = 100;
DCMOTOR(Y).Setting.PwmMin = 50;
DCMOTOR(Y).Setting.PosErrorGain = 1;
DCMOTOR(Y).PosRamp.maxSpeed = 2000;
DCMOTOR(Y).PosRamp.maxAccel = 5000;
DCMOTOR(Y).PosRamp.maxDecel = 5000;
rampSetPos(&DCMOTOR(Y).PosRamp, 16384);
DCMOTOR(Y).PosPID.GainP = 200; //90
DCMOTOR(Y).PosPID.GainI = 1;
DCMOTOR(Y).PosPID.GainD = 0;
DCMOTOR(Y).PosPID.MaxOut = 1023;
DCMOTOR(Y).VolVars.homed = 1;
pinModeDigitalOut(MYEN2);
digitalSet(MYEN2);
#endif
analogInit();
//ANALOG_SELECT(MOTX_POSCHAN, MOTX_POS);
analogSelect(MOTX_CSCHAN, MXCS);
//ANALOG_SELECT(MOTY_POSCHAN, MOTY_POS);
analogSelect(MOTY_CSCHAN, MYCS);
analogSelect(ACCX_CHAN, ACCX);
analogSelect(ACCZ_CHAN, ACCZ);
switchSelect(0,K7);
//EEreadMain();
delayStart(mainDelay, 5000); // init the mainDelay to 5 ms
}
void loop() {
//ADService();
fraiseService();
analogService();
fraiseService();
//Servo_Service();
fraiseService();
switchService();
/*if(Elapsed(servoSOF)>Micros(18000UL)) {
Servo_Rewind();
servoSOF=GetTime();
}*/
if(delayFinished(mainDelay)) // when mainDelay triggers
{
delayStart(mainDelay, 5000); // re-init mainDelay
//DCMOT_GETPOS(X) = Analog_Get(0);
if(!switchSend()) analogSend();
/*anVal = Analog_Get(MOTX_POSCHAN);
if((anVal<11000) || (anVal>21000)) {
DCMOT(X).Vars.PWMConsign = 0;
DCMOT(X).Setting.Mode = 0;
}
else DCMOT_GETPOS(X) = anVal;
anVal = Analog_Get(MOTY_POSCHAN);
if((anVal<11000) || (anVal>21000)) {
DCMOT(Y).Vars.PWMConsign = 0;
DCMOT(Y).Setting.Mode = 0;
}
else DCMOT_GETPOS(Y) = anVal;
*/
wdService();
if(!wdOK()) {
DCMOTOR(X).Vars.PWMConsign = 0;
DCMOTOR(X).Setting.Mode = 0;
DCMOTOR(Y).Vars.PWMConsign = 0;
DCMOTOR(Y).Setting.Mode = 0;
}
DCMOTOR_COMPUTE_SINGLE(X, SYM);
DCMOTOR_COMPUTE(Y, SYM);
fraiseService();
//UpdateMotorD();
//DCMOT_COMPUTE(D, ASYM);
//FraiseService();
//ADService();
//UpdateMotorB();
//FraiseService();
//ADService();
//HMC5883_service();
if(!t--){
t=PERIOD;
t2++;
printf("C M %ld %ld %d\n",DCMOTOR_GETPOS(X),(long)(DCMOTOR(X).PosRamp.currentPos>>(RAMP_UINCPOW)),DCMOTOR(X).VolVars.homed);
//printf("C M %d %d %d\n",HMC5883_getX(),HMC5883_getY(),HMC5883_getZ());
/*len = 0;
buf[len++] = 'B';
buf[len++] = 200;
buf[len++] = IRB1;
buf[len++] = IRB2;
buf[len++] = '\n';
FrTXPacket(buf,len);*/
#if 0
if(t2%2) printf("C MX %ld %ld %d %d\n",DCMOT_GETPOS(X),(long)(DCMOT(X).PosRamp.ConsignPos>>RAMP_UINCPOW), DCMOT(X).Vars.PWMConsign,DCMOT(X).Setting.Mode/*.VolVars.homed*/);
/*else printf("C MD %ld %ld %d %d\n",DCMOT_GETPOS(D),(long)(DCMOT(D).PosRamp.ConsignPos>>RAMP_UINCPOW), DCMOT(D).Vars.PWMConsign,DCMOT(D).VolVars.homed);*/
#endif
}
}
void BattleController::onViewEnter()
{
Controller::onViewExit();
delayStart();
}
