#!/usr/bin/env python2
# -*- coding: utf-8 -*-
"""
Created on Mon Mar  9 14:35:11 2020

@author: Ethan
"""

import rospy
from pololu_drv8835_rpi import motors  	# MAX_SPEED is 480 (hard-coded)

'''
Future work: write these in yaml file
'''
speed_desired = 0.5 # desired wheel speed in rpm
angle_desired = 0.0 # desired angle - 0
k_p_angle = 4*480.0/90.0 # propotional gain for angle control
k_i_angle = k_p_angle/4. # integral gain for angle control
k_d_angle = 1 # derivatibe gain for angle control
k_p_speed = 15 # proportional gain for speed control (60)
k_i_speed = 35 # integral gain for speed control(30)

time_old= 0.0
time_current = rospy.get_rostime()

def talker():
    # Launch a node called "controller_node"
    rospy.init_node('controller_node', anonymous=False)
    # Subscribe to encoder data
    wheel_speed = rospy.Subscriber('/Encoder_data', Float32, update_current_wheel_speed)
    # Subscribe to imu angle data
    imu_angle = rospy.Subscriber('/IMU_angle', Float32,update_imu_angle)
    #PID control
    PID_control()
    

def update_current_wheel_speed(msg_in):
    global current_wheel_speed
    current_wheel_speed = msg_in.data
    
def update_imu_angle(msg_in):
    global current_imu_angle
    current_imu_angle = msg_in.data
    
def PID_control():
    global current_wheel_speed
    global current_imu_angle
    speed_error_cum = 0.0
    
    
    while True:
        
        #### time update
        time_old = time_current # set previous time reading
        time_current = rospy.get_rostime() # set current time reading
        dt = time_current - time_old # time step
        
        # P
        speed_error = speed_desired - current_wheel_speed
        
        # I
        speed_error_cum += speed_error * dt
        
        # Effort
        # not sure what speed_direction_comp is used for
        angle_desired = 1 * (k_p_speed * speed_error + k_i_speed * speed_error_cum)
    
    

    
