void Tracks_Task(){ for(;;){ // P Controller set_left_speed((left_goal - count_left)*P); set_right_speed((right_goal - count_right)*P); vTaskDelay(10); } }
//Set speed of the both motors // arg: // speed-> 0-255 int set_speed(int speed) { if(speed >255) speed =255; else if(speed <0) speed =0; set_left_speed(speed); pi_sleep(100); set_right_speed(speed); return 1; }