#!/usr/bin/env python
import math
import rospy
from motor_control import RasPiRCCar
from ackermann_msgs.msg import AckermannDriveStamped

class ROSCarContoller():
    def __init__(self):
        settings = {'motor_dir_pin': 26,
                    'motor_pwm_pin': 12,
                    'steering_pwm_pin': 18}

        self.car = RasPiRCCar(**settings)

        rospy.init_node('car_controller', anonymous=True)
        rospy.Subscriber("ackermann_cmd", AckermannDriveStamped, self.callback)
        rospy.spin()

    def callback(self, data):
        speed = data.drive.speed
        angle = math.degrees(data.drive.steering_angle)
        
        self.car.drive(speed, angle)

if __name__ == '__main__':
    try:
        c = ROSCarContoller()
    except rospy.ROSInterruptException:
        pass