Hallo Leute,
dank euer Hilfe ich mit nun die Motorsteuerung gelungen. Mein Roboter kann jetzt folgendes Kunststück:
1) Vier mal 1.6 Meter weit gerade (!) vor und zurück fahren. Wobei ich 200mm/sec für
die Vorwärts-Fahrt angegeben habe, und 400mm/sec für die Rückwärts fahrt.
2) 1.0 Meter vorwärts fahren.
3) Auf der Stelle umdrehen (180°).
4) Wieder zurück fahren (also wieder vorwärts 1.0 Meter).
5) Eine Rechtskurve fahren, mit innenRadius 12cm und Außenradius 24cm. Fahrstrecke
innen 1.5 Meter, außen 3.0 Meter.
Es wird meine einfache nibobee Library benötigt, die ich hier schon veröffentlicht habe, und auch auf meiner Homepage.
Nächster Schritt: Die drive() Funktion so zerlegen, dass sie nicht mehr blocking arbeitet sondern Teil einer Endlosschleife wird. Das dürfte einfach werden.
Code:#include "nibobee.h" #include <util/delay.h> // Wait for start signal (touch any sensor) // While waiting, display debug information from sensors: // LED0: Left odometer sensor // LED3: Right odometer sensor // LED1: System timer // LED2: Center line sensor void wait_for_start() { while (!(SENS_SW1 || SENS_SW2 || SENS_SW3 || SENS_SW4)) { // Display status of odometry sensors while waiting set_LED0(ODO_L); set_LED3(ODO_R); // Display system timer (flashes every second) set_LED1((system_time() % 1000)==0); // Display line sensor set_LED2(analog(LINE_C)>600); } set_LED0(0); set_LED1(0); set_LED2(0); set_LED3(0); _delay_ms(10); while ((SENS_SW1 || SENS_SW2 || SENS_SW3 || SENS_SW4)) {} _delay_ms(400); } // Drive an exact distance and accellerate/decellerate softly to the given speed. // The distance of both wheels can be different. // Speed control works best in range 100-800. double pwm_l; double pwm_r; void drive(int32_t distance_mm_l, int32_t distance_mm_r, uint32_t speed_mm_sec) { #define ODOMETER_TICK_MM 6 // Distance of a single odometer tick in mm #define MOTOR_START_PWM 150 // Minimum PWM value to start the motors #define MOTOR_CONTROL_INTERVAL 200 // Interval of motor control. // Steering control does not work, if the interval is too small. #define SPEED_CONTROL_FACTOR 5 // Factor for speed correction. // Too high value causes slipping, much too high // value causes bucking. #define STEERING_CONTROL_FACTOR 0.3 // Factor forsteering correction. // Too high value causes swinging. // Set motor direction and remove sign from the distance value set_DIR_L(distance_mm_l > 0); set_DIR_R(distance_mm_r < 0); if (distance_mm_l<0) distance_mm_l *= -1; if (distance_mm_r<0) distance_mm_r *= -1; // Calculate the destination distance in odometer ticks uint32_t dest_distance_l=distance_mm_l/ODOMETER_TICK_MM; uint32_t dest_distance_r=distance_mm_r/ODOMETER_TICK_MM; // Calculate the steering factor of left to right motor speed double steering_factor; if (distance_mm_l>0 && distance_mm_r>0) steering_factor=(double) distance_mm_l / (double) distance_mm_r; else steering_factor=1; // Calculate the destination speed in odometer ticks per millisecond double dest_speed=(double) speed_mm_sec/ODOMETER_TICK_MM/1000; // Initialize variables reset_odometer(); uint32_t last_odo_l=0; uint32_t last_odo_r=0; uint32_t last_time=system_time(); // Start the motors, if necessary if (distance_mm_l>0 && PWM_L==0) pwm_l=MOTOR_START_PWM; if (distance_mm_r>0 && PWM_R==0) pwm_r=MOTOR_START_PWM; // Drive until both motors reached the nominal distance while (odometer_left()<dest_distance_l || odometer_right()<dest_distance_r) { // Calculate time interval uint32_t now=system_time(); int32_t interval_time=now-last_time; if (interval_time>=MOTOR_CONTROL_INTERVAL) { // Get current distance in odometer ticks uint32_t odo_l=odometer_left(); uint32_t odo_r=odometer_right(); double delta_odo_l=(odo_l-last_odo_l); double delta_odo_r=(odo_r-last_odo_r); // average speed of both wheels in odometer ticks per millisecond double distance=(delta_odo_l+delta_odo_r)/2; double speed=distance/interval_time; double speed_error=dest_speed-speed; // Calculate steering error double steering_error=0; if (delta_odo_r>0 && delta_odo_l>0) steering_error=steering_factor-(delta_odo_l/delta_odo_r); // Display speed status set_LED1(speed_error>0); set_LED2(speed_error>0); // Display steering status set_LED0(steering_error<0); set_LED3(steering_error>0); // Speed control with if (speed_error!=0) { pwm_l+=speed_error*interval_time*SPEED_CONTROL_FACTOR*steering_factor; pwm_r+=speed_error*interval_time*SPEED_CONTROL_FACTOR/steering_factor; } // Steering control with if (steering_error!=0) { pwm_l+=steering_error*interval_time*STEERING_CONTROL_FACTOR; pwm_r-=steering_error*interval_time*STEERING_CONTROL_FACTOR; } // Limit PWM values to the valid range if (pwm_l>1023) pwm_l=1023; if (pwm_r>1023) pwm_r=1023; if (pwm_l<0) pwm_l=0; if (pwm_r<0) pwm_r=0; // Apply the new PWM values PWM_L=pwm_l; PWM_R=pwm_r; last_time=now; last_odo_l=odo_l; last_odo_r=odo_r; } } } // Stop driving. void stop() { PWM_L=0; PWM_R=0; set_LED0(1); set_LED1(1); set_LED2(1); set_LED3(1); _delay_ms(50); set_LED0(0); set_LED1(0); set_LED2(0); set_LED3(0); _delay_ms(50); set_LED0(1); set_LED1(1); set_LED2(1); set_LED3(1); _delay_ms(50); set_LED0(0); set_LED1(0); set_LED2(0); set_LED3(0); _delay_ms(50); } // Main program int main() { wait_for_start(); // Drive a few time forward and backward for 1,6 meters for (int i=0; i<4; i++) { drive(1300,1300,200); drive(300,300,100); stop(); _delay_ms(300); drive(-1000,-1000,400); drive(-600,-600,100); stop(); _delay_ms(300); } // Drive forward for 1 meter drive(700,700,200); drive(300,300,100); stop(); _delay_ms(300); // Turn 180° drive(180,-180,200); stop(); _delay_ms(300); // Drive back drive(700,700,200); drive(300,300,100); stop(); // Drive a circle drive(3000,1500,200); stop(); return 0; }
Zitieren
Lesezeichen