Sille Van Landschoot
/
RacingRobotsLib
Racing Robots Session
Fork of racing_robots by
Diff: robot_logic.cpp
- Revision:
- 8:c5dccd557aab
- Parent:
- 7:a72215b1910b
diff -r a72215b1910b -r c5dccd557aab robot_logic.cpp --- a/robot_logic.cpp Wed Feb 25 15:56:14 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,163 +0,0 @@ -#include "robot_logic.h" - -// Some defines -#define MAX_SPEED 100 -#define MAX_SENSOR 100 -#define MAX_REAL_SPEED 0.75 - -#define MIN 0 - -// Static scope variables -static m3pi m3pi; - -// Static scope variables for keeping internal state -static int internal_speed = 0; // [-100, +100] -static int internal_turnspeed = 0; // [-100, +100] -static int internal_p = 0; -static int internal_i = 0; -static int internal_d = 0; -static int internal_previous_pos_of_line = 0; -static int internal_led_state = 0; - -void drive(int speed) -{ - if (speed > MAX_SPEED || speed < -MAX_SPEED) { - error("Speed out of range"); - return; - } - - internal_speed = speed; - - if (speed == 0) { - m3pi.stop(); - } else if (speed > 0) { - m3pi.forward(MAX_REAL_SPEED*speed/MAX_SPEED); - } else if (speed < 0) { - m3pi.backward(MAX_REAL_SPEED*(-speed)/MAX_SPEED); - } -} - -void turn(int turnspeed) -{ - if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) { - error("Turn out of range"); - return; - } - internal_turnspeed = turnspeed; - - float left = internal_speed; - float right = internal_speed; - - left -= turnspeed; - right += turnspeed; - - if (right < MIN) - right = MIN; - else if (right > MAX_SPEED) - right = MAX_SPEED; - - if (left < MIN) - left = MIN; - else if (left > MAX_SPEED) - left = MAX_SPEED; - m3pi.left_motor(MAX_REAL_SPEED*left/MAX_SPEED); - m3pi.right_motor(MAX_REAL_SPEED*right/MAX_SPEED); -} - -void stop(void) -{ - m3pi.stop(); -} -void sensor_calibrate(void) -{ - m3pi.sensor_auto_calibrate(); -} - -int line_sensor(void) -{ - // Get position of line [-1.0, +1.0] - float pos = m3pi.line_position(); - return ((int)(pos * MAX_SENSOR)); -} - -void pid_init(int p, int i, int d) -{ - internal_p = p; - internal_i = i; - internal_d = d; -} - -int pid_turn(int line_position) -{ - float derivative, proportional, integral = 0; - float power; - - proportional = line_position / 100.0; - - // Compute the derivative - derivative = line_position - internal_previous_pos_of_line; - - // Compute the integral - integral += proportional; - - // Remember the last position. - internal_previous_pos_of_line = line_position; - - // Compute the power - power = (proportional * (internal_p) ) + (integral*(internal_i)) + (derivative*(internal_d)) ; - - power = power * MAX_SPEED; - if (power < -MAX_SPEED) - power = -MAX_SPEED; - else if (power > MAX_SPEED) - power = MAX_SPEED; - return power ; -} - -void show_stats(void) -{ - m3pi.cls(); // Clear display - - // Display speed - m3pi.locate(0, 0); - m3pi.printf("S%d", internal_speed); - - // Display turn - m3pi.locate(4,0); - m3pi.printf("T%d", internal_turnspeed); - - // Display line - m3pi.locate(0, 1); - int line = line_sensor(); - m3pi.printf("POS %d", line); -} - -void show_name(char * name) -{ - m3pi.cls(); // Clear display - - // Display speed - m3pi.locate(0, 0); - // x The horizontal position, from 0 to 7 - // y The vertical position, from 0 to 1 - m3pi.printf("%s", name); -} - -void await(int milliseconds) -{ - wait_ms(milliseconds); -} - -void led(LedIndex i, LedState state) -{ - if(state == LED_ON) { - internal_led_state |= (1 << i); - } else if(state == LED_OFF) { - internal_led_state &= ~(1 << i); - } else if(state == LED_TOGGLE) { - internal_led_state ^= (1 << i); - } else { - error("Illegal LED state"); - } - m3pi.leds(internal_led_state); -} \ No newline at end of file