void * GPIOPwmPin::runPWM() {
pwmRunning = true;
bool durationExpired = false;
long long durationLeftNS = pwmDuration * 1000000LL;
while (pwmRunning && !durationExpired) {
// HIGH part of cycle
GPIOAccess::rawSet(pinNumber, 1);
sleepNano(periodHighNS);
// LOW part of cycle
GPIOAccess::rawSet(pinNumber, 0);
sleepNano(periodLowNS);
if (pwmDuration > 0) {
durationLeftNS = durationLeftNS - periodHighNS - periodLowNS;
durationExpired = (durationLeftNS <= 0LL);
}
}
if (durationExpired) {
stopPWM();
}
return NULL;
}
void setServoAngle( uint8_t channel, uint8_t angle ){
uint8_t pulseLength = 40 + angle*200/180;
pulseLength = pulseLength*PWM_TIME_PERIOD_BASE/PWM_TIME_PERIOD;
updatePWM(channel, pulseLength);
startPWM();
timer_delay_ms(2000);
stopPWM();
timer_delay_ms(2000);
}
GPIO_Result GPIOPwmPin::setPWM(long int freq, int duty, bool isTone, int durationMs) {
if (freq <= 0) {
return stopPWM();
}
GPIO_Direction dir = GPIOAccess::getDirection(pinNumber);
GPIO_Result res = GPIOAccess::getLastResult();
if (res != GPIO_OK) {
return res;
}
if ((dir != GPIO_OUTPUT) || ((!isTone) && ((duty < 0) || (duty > 100)))) {
return GPIO_INVALID_OP;
}
pwmFreq = freq;
if (!isTone) {
pwmDuty = duty;
} else {
pwmDuty = 50;
}
pwmDuration = durationMs;
pwmIsTone = isTone;
// find the period in nano seconds
long long periodNS = 1000000000LL / (((long long)pwmFreq));
// find the low and high periods based on the duty-cycle
periodHighNS = (periodNS * (long long)pwmDuty) / 100LL;
periodLowNS = periodNS - periodHighNS; //can also be: period * ((100 - dutyCycle) /100);
if (pwmRunning) {
stopPWM();
}
pthread_create(&pwmThread, NULL, &GPIOPwmPin::pwmThreadRunner, this);
return GPIO_OK;
}
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void) {
IFS0bits.T1IF = 0;
if (listening) {
if (timer1_counter == 50) {
timer1_counter = 0;
listening = false;
send_ping = true;
RTT1_received = false;
RTT2_received = false;
RTT1_overflows = 0;
RTT2_overflows = 0;
RTT1_time = 0;
RTT2_time = 0;
stopTimerRTT();
timer2_counter = 0;
// IEC0bits.ADIE = 0;
IFS0bits.ADIF = 0;
ADSTATbits.P0RDY= 0;
} else {
++timer1_counter;
}
} else if (ping_send) {
stopPWM();
startTimerRTT();
ping_send = false;
blocking_sensors = true;
} else if (send_ping) {
initPWM();
startPWM(); // Sent for 1ms
send_ping = false;
ping_send = true;
} else if (blocking_sensors && timer1_counter == 15) { // Blocked during 15ms
timer1_counter = 0;
blocking_sensors = false;
listening = true;
/* Launch ADC */
// IEC0bits.ADIE = 1;
ADCPC0bits.SWTRG0 = 1;
} else {
++timer1_counter;
}
}
GPIOPwmPin::~GPIOPwmPin(void)
{
stopPWM();
}
