Submitted version of the thesis - Airlab, the Artificial Intelligence ...

100 Appendix B. Documentation of the programming
u8 motor2_speed;
u8 motor3_speed;
/* Private function prototypes -----------------------------------------------*/
void Init_Motors(void);
void Calculate_Order(s16* speed, u8 size);
void Position_Camera(u8 position);
void Set_Directions(u8 Motor1, u8 Motor2, u8 Motor3);
void Stop_Motor(u8 Motor);
void Bumpers(u8 bumper[6]);
void Set_Speed(s16* speed, u8 mod);
void Stop_Servo(void);
void Turn(u8 direction);
/* Private functions ---------------------------------------------------------*/
void Set_Speed(s16* speed, u8 mod) {
u8 direction[NUM_MOTORS];
u8 k;
for (k = 0; k < NUM_MOTORS; k++) {
direction[k] = (speed[k] > 0) ? 1 : 0, speed[k]
=Abs(speed[k]);
}
for (k = 0; k < NUM_MOTORS; k++) {
}
if (speed[k] == 0) {
Stop_Motor(k);
}
if (speed[k] > 2500)
speed[k] = 2500;
for (k = 0; k < NUM_MOTORS; k++) {
if (speed[k] != 0)
speed[k] = (speed[k] / 75 * mod) + 90;
}
Set_Directions(direction[0], direction[1], direction[2]);
motor1_speed = (u8) speed[0];
motor2_speed = (u8) speed[1];
motor3_speed = (u8) speed[2];
#ifdef NO_DEBUG
Print_Int(motor1_speed,"m1 applied");
Print_Int(motor2_speed,"m2 applied");
Print_Int(motor3_speed,"m3 applied");