Rover PWM output

rover_pwm_io.py

import sys, time

import navio.rcinput
import navio.pwm
import navio.util

navio.util.check_apm()
PWM_FORWARD_PIN = 13 # Forward to pin labelled 14
PWM_BACKWARD_PIN = 12 # Backwards to pin labelled 13

PWM_FREQUENCY = 50 #Hz frequency

THROTTLE_NEUTRAL = 1500
THROTTLE_MAX = 1900
THROTTLE_MIN = 1100
THROTTLE_DEADZONE = 50

rcin = navio.rcinput.RCInput()

with navio.pwm.PWM(PWM_FORWARD_PIN) as forward, navio.pwm.PWM(PWM_BACKWARD_PIN) as backward:
    forward.set_period(PWM_FREQUENCY)
    backward.set_period(PWM_FREQUENCY)

    while (True):
        throttle = rcin.read(2) # This should be the throttle
        print "Throttle: " + throttle # Display RC throttle value to screen

        forwardDutyCycle, backwardDutyCycle = calculateDutyCycle(throttle)
        print "Forward duty cycle: " + forwardDutyCycle
        print "Backward duty cycle: " + backwardDutyCycle

        forward.set_duty_cycle(forwardDutyCycle)
        backward.set_duty_cycle(backwardDutyCycle)

        time.sleep(0.100) # Wait 100ms before reading throttle again

def calculateDutyCycle(throttle):
    throttleDisplacement = throttle - THROTTLE_NEUTRAL
    if abs(throttleDisplacement) <= THROTTLE_DEADZONE:
        return 0, 0
    elif throttleDisplacement > 0 # Go forward
        speed = throttleDisplacement / (THROTTLE_MAX - THROTTLE_NEUTRAL) 
        return speed * (1000 / PWM_FREQUENCY), 0
    elif throttleDisplacement < 0 # Go backward
        speed = abs(throttleDisplacement) / (THROTTLE_NEUTRAL - THROTTLE_MIN) 
        return 0, speed * (1000 / PWM_FREQUENCY)
    else return 0, 0 # Just in case