Öffentliche Projekte

//Motor

#include <nibobee/delay.h>
#include <nibobee/iodefs_nibobee.h>
#include <nibobee/led.h>
#include <nibobee/motpwm.h>
#include <nibobee/odometry.h>
#include <nibobee/sens.h>
#include <avr/interrupt.h>
#include <nibobee/usart.h>
#include <nibobee/clock.h>
#include <nibobee/analog.h>
#include <nibobee/iodefs.h>
#include <nibobee/line.h>

#include <stdlib.h>

void print_status() {
  
}

float factor_left = 10;
float factor_left_backward = 10;
float factor_right = 10;
float factor_right_backward = 10;
int16_t speed_left_old = 0;
int16_t speed_right_old = 0;
uint16_t zyclus_duration = 100;

//speed: odo-ticks pro Sekunde, Suration: in Millisekunden
void drive(int16_t speed_left, int16_t speed_right, uint16_t duration, uint8_t boost) {
	print_status(";Drive; %5d; %5d; %5u", speed_left, speed_right, duration);
	//Anstossen
	if (boost) {
	if (speed_left_old == 0) {
		motpwm_setLeft(speed_left > 0 ? 800 : -800);
	}
	if (speed_right_old == 0) {
		motpwm_setRight(speed_right > 0 ? 800 : -800);
	}	
		delay(50);
	}

//Wert holen
	speed_left_old = speed_left;
	speed_right_old = speed_right;
	odometry_getLeft(1);
	odometry_getRight(1);
	//eigentliche Regelschleife
	int16_t target_ticks_left = speed_left / (1000 / zyclus_duration);
	int16_t target_ticks_right = speed_right / (1000 / zyclus_duration);
	int16_t duration_ticks = duration / zyclus_duration;
	float factor_left_intern = factor_left;
	float factor_right_intern = factor_right;
	int16_t error_left[3] = {0,0,0};
	int16_t error_right[3] = {0,0,0};
	while (duration_ticks > 0) {
		//Wert setzen
		int16_t motpwm_left_value = speed_left * factor_left_intern;
		if (motpwm_left_value > 1023) {
			motpwm_left_value = 1023;
		}
		if (motpwm_left_value < -1023) {
			motpwm_left_value = -1023;
		}
		motpwm_setLeft(motpwm_left_value);

int16_t motpwm_right_value = speed_right * factor_right_intern;
		if (motpwm_right_value > 1023) {
			motpwm_right_value = 1023;
		}
		if (motpwm_right_value < -1023) {
			motpwm_right_value = -1023;
		}
		motpwm_setRight(motpwm_right_value);

duration_ticks--;
		delay(zyclus_duration);

int16_t actual_ticks_left = odometry_getLeft(1);
		if (actual_ticks_left != 0) {
			float correction_factor = (float)target_ticks_left / actual_ticks_left;
			if (correction_factor > 1.2) {
				correction_factor = 1.2;
			}
			if (correction_factor < 0.8) {
				correction_factor = 0.8;
			}
			factor_left_intern *= correction_factor;
			if (factor_left_intern < 3) {
				factor_left_intern = 3;
			}
		} else {
			factor_left_intern *= 1.5;
		}
		if (factor_left_intern > 50) {
			factor_left_intern = 50;
		}
		if (factor_left_intern <= 0) {
			factor_left_intern = 3;
		}

int16_t actual_ticks_right = odometry_getRight(1);
		if (actual_ticks_right != 0) {
			float correction_factor = (float)target_ticks_right / actual_ticks_right;
			if (correction_factor > 1.2) {
				correction_factor = 1.2;
			}
			if (correction_factor < 0.8) {
				correction_factor = 0.8;
			}
			factor_right_intern *= correction_factor;
			if (factor_right_intern < 3) {
				factor_right_intern = 3;
			}
		} else {
			factor_right_intern *= 1.5;
		}
		if (factor_right_intern > 50) {
			factor_right_intern = 50;
		}
		if (factor_right_intern <= 0) {
			factor_right_intern = 3;
		}

error_left[0] = error_left[1];
		error_left[1] = error_left[2];
		error_left[2] = target_ticks_left - actual_ticks_left;
		print_status("l;pwm; %5d; fact; %8ld; tar; %4d; act; %4d", motpwm_left_value, (int32_t)(factor_left_intern * 1000), target_ticks_left, actual_ticks_left);

error_right[0] = error_right[1];
		error_right[1] = error_right[2];
		error_right[2] = target_ticks_right - actual_ticks_right;
		print_status("r;pwm; %5d; fact; %8ld; tar; %4d; act; %4d", motpwm_right_value, (int32_t)(factor_right_intern * 1000), target_ticks_right, actual_ticks_right);
	}
	//kann man das als neuen Faktor nehmen?
	int16_t sum_error_left = abs(error_left[0]) + abs(error_left[1]) +abs(error_left[2]);
	if (sum_error_left <= 5 ) {
		print_status("guter Faktor");
		factor_left = factor_left_intern;
	}
	int16_t sum_error_right = abs(error_right[0]) + abs(error_right[1]) +abs(error_right[2]);
	if (sum_error_right <= 5 ) {
		print_status("guter Faktor");
		factor_right = factor_right_intern;
	}
}

int main() {

motpwm_init();
	usart_enable();
	usart_setbaudrate(38400);
	motpwm_init();
	odometry_init();
	sei();
	print_status("hallo");
	while(1) {
		drive(80,80,1000,1);
		drive(40,40,1000,0);
		drive(80,40,1000,0);
		drive(40,80,1000,0);
		motpwm_setRight(0);
		motpwm_setLeft(0);
		while(1);
	}
	print_status("fertig");
	while(1);
}

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

motorregler_float/main.c [read only]

Werbung

Online