#!/usr/bin/python

import time
import math
import Adafruit_BBIO.GPIO as GPIO
import Adafruit_BBIO.PWM as PWM
import ConstantsFile

Pconst = ConstantsFile.Constants()

# Start the GPIO's

#PWM.start(channel, duty, freq, polarity)
PWM.start(Pconst.PWM_R_PWM, 0, Pconst.PWM_Freq) #pwm starts disabled
PWM.start(Pconst.PWM_L_PWM, 0, Pconst.PWM_Freq) #pwm starts disabled
GPIO.setup(Pconst.L_POS, GPIO.OUT)
GPIO.setup(Pconst.L_NEG, GPIO.OUT)
GPIO.setup(Pconst.R_POS, GPIO.OUT)
GPIO.setup(PConst.R_NEG, GPIO.OUT)

class PWMClass ():
	
	def PWMStart(self):
    	    # Starting the communication with the PWM controller
            #GPIO.output(Pconst.RedLED, GPIO.HIGH)
            time.sleep(3)						# Wait to stabilize the communication
	    
	def PWM_Signals(self, rightMotor, leftMotor):

	    # Sending the values to the PWM controller that is connected to the motors
	    percentageR=math.fabs(rightMotor)
	    percentageL=math.fabs(leftMotor)
	    
	    if rightMotor>0:
		#PWM.set_duty_cycle(Pconst.PWM_R_DIR, 100)
		GPIO.output(Pconst.R_POS, GPIO.HIGH)
		GPIO.output(Pconst.R_NEG, GPIO.LOW)
		PWM.set_duty_cycle(Pconst.PWM_R_PWM, percentageR)
	    elif rightMotor<0:
		#PWM.set_duty_cycle(Pconst.PWM_R_DIR,  0)
		GPIO.output(Pconst.R_POS, GPIO.LOW)
		GPIO.output(Pconst.R_NEG, GPIO.HIGH)
		PWM.set_duty_cycle(Pconst.PWM_R_PWM,  percentageR)
	    elif rightMotor==0:
		PWM.set_duty_cycle(Pconst.PWM_R_PWM, 0)
		GPIO.output(Pconst.R_POS, GPIO.LOW)
		GPIO.output(Pconst.R_NEG, GPIO.LOW)
    	    if leftMotor>0:
	        #PWM.set_duty_cycle(Pconst.PWM_L_DIR, 0)
		GPIO.output(Pconst.L_POS, GPIO.HIGH)
		GPIO.output(Pconst.L_NEG, GPIO.LOW)
	        PWM.set_duty_cycle(Pconst.PWM_L_PWM, percentageL)
	    elif leftMotor<0:
	    	#PWM.set_duty_cycle(Pconst.PWM_L_DIR, 100)
		GPIO.output(Pconst.L_POS, GPIO.LOW)
		GPIO.output(Pconst.L_NEG, GPIO.HIGH)
      	    	PWM.set_duty_cycle(Pconst.PWM_L_PWM, percentageL)
    	    elif leftMotor==0:
	   	PWM.set_duty_cycle(Pconst.PWM_L_PWM, 0)
		GPIO.output(Pconst.L_POS, GPIO.LOW)
		GPIO.output(Pconst.L_NEG, GPIO.LOW)

			
	def PWMStop(self):
	    	PWM.stop(Pconst.PWM_R_PWM)
		GPIO.output(Pconst.R_POS, GPIO.LOW)
		GPIO.output(Pconst.R_NEG, GPIO.LOW)		
		PWM.stop(Pconst.PWM_L_PWM)
		GPIO.output(Pconst.L_POS, GPIO.LOW)
		GPIO.output(Pconst.L_NEG, GPIO.LOW)
		PWM.cleanup()
		
		
