Now resetting alignment i component when the robot is aligned the other way (the sign of w is changed). Commented out the i treshold part