/**
* Angle manager function
* Correcteur proportionnel integral
*
* \param error : the mesured error to correct using this corrector
*
* /!\ Time between each call of the function is a factor of the resulting angle,
* it's strongly recommended to use a timer to manage the time between each
* call.
*
*/
void angle_corrector_v1(char error){
//Computing
angle_sum += ANGLE_SUM_FACTOR*(error);
//Checking if it's still in the max and min defined
if(angle_sum > ANGLE_INTEGRATOR_SUM_MAX){
angle_sum = ANGLE_INTEGRATOR_SUM_MAX;
}
else if(angle_sum < ANGLE_INTEGRATOR_SUM_MIN){
angle_sum = ANGLE_INTEGRATOR_SUM_MIN;
}
angle_order = ANGLE_PROPORTIONNAL_FACTOR*(error) + ANGLE_INTEGRATOR_FACTOR*angle_sum; //
//Limiting the angle value (physically limited)
if((angle_order <= 35) && (angle_order >= -35)){
set_servo_angle(angle_order);
}
else if(angle_order<-35){
set_servo_angle(-35);
}
else if(angle_order>35){
set_servo_angle(35);
}
}
void angle_manager(void){
if(line.found == 1){
angle_corrector((char)line.position - 64);
}
else{
if(line.last_direction == right){
set_servo_angle(ANGLE_NO_LINE);
//angle_sum = ANGLE_INTEGRATOR_SUM_MAX;
}
else if(line.last_direction == middle){
set_servo_angle(0);
//angle_sum = 0;
}
else{
set_servo_angle(-ANGLE_NO_LINE);
//angle_sum = ANGLE_INTEGRATOR_SUM_MIN;
}
}
}
void angle_corrector(char error){
static float speedCorrector_coeff[10] = {
ANGLE_PROPORTIONNAL_10,
ANGLE_PROPORTIONNAL_20,
ANGLE_PROPORTIONNAL_30,
ANGLE_PROPORTIONNAL_40,
ANGLE_PROPORTIONNAL_50,
ANGLE_PROPORTIONNAL_60,
ANGLE_PROPORTIONNAL_70,
ANGLE_PROPORTIONNAL_80,
ANGLE_PROPORTIONNAL_90,
ANGLE_PROPORTIONNAL_100
};
float current_proportionnal_factor = 0;
if (speedDutyLeft <= 10 ) {
current_proportionnal_factor = speedCorrector_coeff[0];
}else if (speedDutyLeft > 10 && speedDutyLeft <= 20) {
current_proportionnal_factor = speedCorrector_coeff[1];
}else if (speedDutyLeft > 20 && speedDutyLeft <= 30) {
current_proportionnal_factor = speedCorrector_coeff[2];
}else if (speedDutyLeft > 30 && speedDutyLeft <= 40) {
current_proportionnal_factor = speedCorrector_coeff[3];
}else if (speedDutyLeft > 40 && speedDutyLeft <= 50) {
current_proportionnal_factor = speedCorrector_coeff[4];
}else if (speedDutyLeft > 50 && speedDutyLeft <= 60) {
current_proportionnal_factor = speedCorrector_coeff[5];
}else if (speedDutyLeft > 60 && speedDutyLeft <= 70) {
current_proportionnal_factor = speedCorrector_coeff[6];
}else if (speedDutyLeft > 70 && speedDutyLeft <= 80) {
current_proportionnal_factor = speedCorrector_coeff[7];
}else if (speedDutyLeft > 80 && speedDutyLeft <= 90) {
current_proportionnal_factor = speedCorrector_coeff[8];
}else if (speedDutyLeft > 90 && speedDutyLeft <= 100) {
current_proportionnal_factor = speedCorrector_coeff[9];
}else {
current_proportionnal_factor = ANGLE_PROPORTIONNAL_FACTOR;
}
//Computing
angle_sum += ANGLE_SUM_FACTOR*(error);
//Checking if it's still in the max and min defined
if(angle_sum > ANGLE_INTEGRATOR_SUM_MAX){
angle_sum = ANGLE_INTEGRATOR_SUM_MAX;
}
else if(angle_sum < ANGLE_INTEGRATOR_SUM_MIN){
angle_sum = ANGLE_INTEGRATOR_SUM_MIN;
}
angle_order = current_proportionnal_factor*(error);// + ANGLE_INTEGRATOR_FACTOR*angle_sum
//Limiting the angle value (physically limited)
if((angle_order <= 30) && (angle_order >= -30)){
set_servo_angle(angle_order);
}
else if(angle_order < -30){
set_servo_angle(-30);
}
else if(angle_order > 30){
set_servo_angle(30);
}
}
void ppm_decode(void){
SerialUSB.println("Decoding DIY Drones PPM encoder on pin D27");
int pin = 16;
uint8 prev_state;
int time_elapsed = 0;
int time_start = 0;
int i = 0;
int channels[8];
float angle;
pinMode(pin, INPUT);
prev_state = (uint8)digitalRead(pin);
while(!SerialUSB.available()){
uint8 current_state = (uint8)digitalRead(pin);
if (current_state != prev_state) {
if (current_state) {
time_elapsed = (micros() - time_start);
time_start = micros();
}else{
#ifdef USB_VERBOSE
SerialUSB.print(i);
SerialUSB.print(":");
SerialUSB.print(time_elapsed);
#endif
if(time_elapsed < 2500 && i < 8){
if(i==2){
if(time_elapsed < 1000){
angle = 0.0;
}else if(time_elapsed > 2000){
angle = 90.0;
}else{
angle = (float)((time_elapsed-1000)*PPM_CNTS_TO_DEG);
}
SerialUSB.print(":");
SerialUSB.print(angle);
set_servo_angle(angle);
}
channels[i++] = time_elapsed;
}else{
#ifdef USB_VERBOSE
SerialUSB.println("");
#endif
i=0;
}
#ifdef USB_VERBOSE
SerialUSB.print("\t");
#endif
time_elapsed = 0;
}
prev_state = current_state;
}
}
SerialUSB.println("Done!");
}
void USART2_IRQHandler (void)
{
//Приём байта
if ( USART_GetITStatus(USART2, USART_IT_RXNE) != RESET )
{
char uart_data = USART2->DR;
//чистим флаг прерывания
USART_ClearITPendingBit(USART2, USART_IT_RXNE);
uart2_rx_buf[uart2_rx_bit] = uart_data;
++;
//завершение приёма пакета
if ( (uart2_rx_buf[0] + 2) == uart2_rx_bit )
{
uart2_rx_bit = 0;
switch (uart2_rx_buf[1])
{
//эхо
case CMD_ECHO:
{
break;
}
//установка позиции.
case CMD_SET_SEVO_POS:
{
if ( uart2_rx_buf[0] == sizeof(struct ServoPosData))
uart2_rx_buf[2] = set_servo_angle(&servos_data, (struct ServoPosData *)&uart2_rx_buf[2]);
else
uart2_rx_buf[2] = WRONG_CMD_PACKET_SIZE;
uart2_rx_buf[0] = 1;
break;
}
case CMD_SET_SEVOS_RANGES:
{
if ( uart2_rx_buf[0] == sizeof(struct GroupSettings))
uart2_rx_buf[2] = set_servo_range(&servos_data, (struct GroupSettings *)&uart2_rx_buf[2]);
else
uart2_rx_buf[2] = WRONG_CMD_PACKET_SIZE;
uart2_rx_buf[0] = 1;
break;
}
case CMD_ADD_ANIMATION:
{
uart2_rx_buf[0] = 1;
uart2_rx_buf[2] = NOT_IMPLEMENTED;
break;
}
case CMD_CLEAR_ANIMATION:
{
uart2_rx_buf[0] = 1;
uart2_rx_buf[2] = NOT_IMPLEMENTED;
break;
}
case CMD_START_ANIMATIONS:
{
uart2_rx_buf[0] = 1;
uart2_rx_buf[2] = NOT_IMPLEMENTED;
break;
}
case CMD_STOP_ANIMATIONS:
{
uart2_rx_buf[0] = 1;
uart2_rx_buf[2] = NOT_IMPLEMENTED;
break;
}
case CMD_GET_SEVOS_RANGES:
{
uart2_rx_buf[0] = 1;
if ( uart2_rx_buf[0] == sizeof(uint8_t) )
{
uart2_rx_buf[2] = get_servo_range(&servos_data,
*((uint8_t *)&uart2_rx_buf[2]),
(struct GroupSettings *)&uart2_rx_buf[3]);
if ( uart2_rx_buf[2] == 0 )
uart2_rx_buf[0] = 1 + sizeof(struct GroupSettings);
}
else
uart2_rx_buf[2] = WRONG_CMD_PACKET_SIZE;
break;
}
}
send_buffer(uart2_rx_buf[0] + 2, uart2_rx_buf);
}
}