Sille Van Landschoot
/
racing_robots
Racing Robots Session
Fork of racing_robots by
robot_logic.cpp
- Committer:
- pcordemans
- Date:
- 2015-02-25
- Revision:
- 6:0dc4e4225881
- Parent:
- 5:355240d7126b
- Child:
- 7:a72215b1910b
File content as of revision 6:0dc4e4225881:
#include "robot_logic.h" // Some defines #define MAX_SPEED 100 #define MAX_SENSOR 100 #define MAX_REAL_SPEED 0.3 #define MAX 0.3 #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) { printf("[ERROR] Drive 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("Speed 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 right; float left; float derivative, proportional, integral = 0; float power; float speed = internal_speed; proportional = line_position; // 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)) ; return power; } /** *Show speed, turn and sensor data */ 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); }