import smbus
import time
import RPi.GPIO as GPIO
import pid

#Speed
SPEED = 50

P = 2
I = 0
D = 0

#IO 
PWM_PIN = 14
PWM_FREQ = 50

#ADC
I2C_ADDR = 0x48


GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(PWM_PIN, GPIO.OUT)

pwm = GPIO.PWM(PWM_PIN, PWM_FREQ)
pwm.start(0)
pwm.ChangeDutyCycle( 70 )

bus = smbus.SMBus(1)

controller = pid.PID(2,0.1,0)

controller.setSetPoint( 30 )

try:
  while True:
    # ADC 0
    bus.write_byte(I2C_ADDR, 0x40)
    bus.read_byte(I2C_ADDR)           # dummy read to start conversion
    sensor = bus.read_byte(I2C_ADDR)

    control_signal = controller.update( sensor )

    if control_signal < 0:
      control_signal = 0

    if control_signal > 100:
      control_signal = 100

    #outputString = str(sensor) + " " + str(control_signal) + " " + str(controller.PTerm) + " " + str(controller.ITerm)
    #print outputString      

    pwm.ChangeDutyCycle( control_signal )
    
    time.sleep(0.001)

except KeyboardInterrupt:
	pass
pwm.stop()
GPIO.cleanup()