#!/usr/bin/env python
# Revision $Id$

## Simple talker demo that listens to std_msgs/Strings published
## to the 'chatter' topic

import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from time import sleep
import RPi.GPIO as GPIO
r = 0.01
velocity = 75
en = 25
temp1=1
class Motor():
	def __init__(self, p1, p2):
		self.p1 = p1
		self.p2 = p2
		gpio.setup(p1, gpio.OUT)
		gpio.setup(p2, gpio.OUT)
	#x != 0 z = 0
	def runlinear(self, data):
		#Negatieve snelheid
		if data.linear.x < 0:
			GPIO.output(self.p1, False)
			GPIO.output(self.p2, True)
		#positieve snelheid
		elif data.linear.x > 0:
			GPIO.output(self.p1, True)
			GPIO.output(self.p2, False)
		#de x is nul => traag afbouwen
		else:
			p.ChangeDutyCycle(65)
			sleep(0.5)
			p.ChangeDutyCycle(50)
			sleep(0.5)
			p.ChangeDutyCycle(25)
			sleep(0.5)
			GPIO.output(self.p1, False)
			GPIO.output(self.p2, False)
	#x = 0 z !=0
	def turn(self, data):
		#data is de waarde van angular Z en is dus negatief bij een van de twee motoren
		#Als de juiste twist waarden worden doorgegeven wordt bepaald in welke richting gedraait moet worden en dit wordt dan doorgegeven
		#Bv -2 0 0 als angular waarden wordt aan 1 motor -2 gegeven en aan de andere 2
		vel = data.angular.z * r

		if vel > 0:
			GPIO.output(self.p1, True)
			GPIO.output(self.p2, False)
		else:
			#achteruit rijden
			GPIO.output(self.p1, False)
			GPIO.output(self.p2, True)
			vel *= -1
			#vel moet wel positief zijn
		p.ChangeDutyCycle(vel)
	#x =! 0 z !=0 => wiel langs een kant vertragen
	def linearturn(self, data):

		if data.angular.z > 0:
			GPIO.output(self.p1, True)
			GPIO.output(self.p2, False)
			vel = 75 - data.angular.z * r
		else:
			GPIO.output(self.p1, True)
			GPIO.output(self.p2, False)
			#velocity moet > zijn als de andere velocity op het andere wiel, + of - mag enkel bepalen welk wiel vertraagd wordt, niet ervoor zorgen dat
			posvel = data.angular.z * -1
			vel = 75 - posvel * r
			vel *= -1
		p.ChangeDutyCycle(vel)


def conf():
    r = 0.01
    velocity = 95
    en = 25
    temp1=1
    GPIO.setmode(GPIO.BCM)
    GPIO.setup(en, GPIO.OUT)
    p=GPIO.PWM(en,1000)
    p.start(velocity)

def callback(data):
    rospy.loginfo(data.linear.x)
    if data.linear.x != 0:
	    if data.angular.z == 0:
	    		motor_left.runlinear(data)
	    		motor_right.runlinear(data)
	    else:
		if data.angular.z > 0:
		    	motor_left.runlinear(data)
		    	motor_right.linearturn(data)
    else:
	    if data.angular.z == 0:
			motor_left.turn(data)
			motor_right.turn(data)
def listener():

    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber('chatter', Twist, callback)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    try:
	    motor_left = Motor(17,22)
	    motor_right = Motor(23,24)
	    conf()
	    listener()
    except keyboardinterrupt:
	    rospy.loginfo("exit application")