Fix angle calculation
Added sensor control