Zitieren
hallo allerseits,
auch hier kleine fortschritte. Habe das programm für den asuro mit PID-regler doch für den RP6 "umgeschrieben". Um es von der M32 betreiben zu können waren in der "RP6Control_I2CMasterLib.c", der "RP6Control_I2CMasterLib..h" und der "RP6Base_I2CSlave.c" folgende änderungen notwendig:
undCode:set_motor_power_in_base_slave... in der RP6Control_I2CMasterLib.c einfügen: ------------------------------------------ // Speed uint8_t left_power; uint8_t right_power; /** * Set Motor power function */ void setMotorPower(uint8_t left_power, uint8_t right_power) { I2CTWI_transmit4Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_MOTOR_POWER, left_power, right_power ); while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI(); } in der RP6Control_I2CMasterLib.h einfügen: ------------------------------------------ #define CMD_SET_MOTOR_POWER 14 void setMotorPower(uint8_t left_power, uint8_t right_power); // Speed extern uint8_t left_power; extern uint8_t right_power; in der RP6Base_I2CSlave.c einfügen: ---------------------------------- #define CMD_SET_MOTOR_POWER 14 in der Funktion "void task_commandProcessor(void)" einfügen: case CMD_SET_MOTOR_POWER: setMotorPower(param1, param2); break;
der eigentlicherCode:set_motor_dir_in_base_slave... in der RP6Control_I2CMasterLib.c einfügen: ------------------------------------------ // Direction uint8_t left_dir; uint8_t right_dir; /** * Set Motor dir function */ void setMotorDir(uint8_t left_dir, uint8_t right_dir) { I2CTWI_transmit4Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_MOTOR_DIR, left_dir, right_dir ); while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI(); } in der RP6Control_I2CMasterLib.h einfügen: ------------------------------------------ #define CMD_SET_MOTOR_DIR 13 void setMotorDir(uint8_t left_dir, uint8_t right_dir); // Direction extern uint8_t left_dir; extern uint8_t right_dir; in der RP6Base_I2CSlave.c einfügen: ---------------------------------- #define CMD_SET_MOTOR_DIR 13 in der Funktion "void task_commandProcessor(void)" einfügen: case CMD_SET_MOTOR_DIR: setMotorDir(param1, param2); break;
und das ergebnis ist hier,Code:/******************************************************************************* * * Description: Asuro Linienverfolgung mit PID-Regler * Version 1: Korrektur auf beide Motoren verteilt * Author: Waste 26.8.05 * * Version 2: initialization of parameters in Main, measurement with LED off in Main * Author: irobot22587 11.03.2007 * überarbeitet für RP6 inka / 2014_03_10 *****************************************************************************/ #include "RP6ControlLib.h" #include "RP6I2CmasterTWI.h" #include "RP6Control_MultiIOLib.h" #include "RP6Control_I2CMasterLib.h" #include "RP6Control_LFSBumperLib.h" #include "RP6ControlServoLib.h" #include "standard.h" #define I2C_RP6_BASE_ADR 10 unsigned char speed; int speedLeft,speedRight; unsigned int lineData[2]; int x, xalt, don, doff, kp, kd, ki, yp, yd, yi, drest, y, y2, isum; void FollowLine (void) { unsigned char leftDir = FWD, rightDir = FWD; lineData[0] = getLFS(CH_LFS_L); lineData[1] = getLFS(CH_LFS_R); x = (lineData[0] - lineData[1]); isum += x; if (isum > 16000) isum =16000; //Begrenzung um Überlauf zu vermeiden if (isum < -16000) isum =-16000; yi = isum/625 * ki; //I-Anteil berechnen //625 yd = (x - xalt)*kd; // D-Anteil berechnen und mit yd += drest; // nicht berücksichtigtem Rest addieren if (yd > 255) drest = yd - 255; // merke Rest else if (yd < -255) drest = yd + 255; else drest = 0; yp = x*kp; // P-Anteil berechnen y = yp + yi + yd; // Gesamtkorrektur y2 = y/2; // Aufteilung auf beide Motoren xalt = x; // x merken speedLeft = speedRight = speed; setMotorDir(FWD,FWD); if ( y > 0) { // nach rechts setbumperLEDL(1); setbumperLEDR(0); speedLeft = speed + y2; // links beschleunigen if (speedLeft > 120) //255,155,100 { speedLeft = 80; //155,105,50 // falls Begrenzung y2 = speedLeft - speed; // dann Rest rechts berücksichtigen } y = y - y2; speedRight = speed - y; // rechts abbremsen if (speedRight < 0) { speedRight = 0; } } if ( y < 0) { // nach links setbumperLEDR(1); setbumperLEDL(0); speedRight = speed - y2; // rechts beschleunigen !!!was speed - y2!! if (speedRight > 120) //255,105,100 { speedRight = 80; //155,105,50 // falls Begrenzung y2 = speed - speedRight; // dann Rest links berücksichtigen !!was speed - speedRight!! } y = y - y2; speedLeft = speed + y; // links abbremsen !! was speed + y!!! if (speedLeft < 0) { speedLeft = 0; } } leftDir = rightDir = FWD; if (speedLeft < 20) mleft_speed = 0; if (speedRight < 20) mright_speed = 0; setMotorDir(leftDir,rightDir); setMotorPower(abs(speedLeft),abs(speedRight)); } int main(void) { initRP6Control(); multiio_init(); initLCD(); lfsbumper_init(); I2CTWI_initMaster(100); I2CTWI_setTransmissionErrorHandler(I2C_transmissionError); setLEDs(0b1111); clearLCD(); setCursorPosLCD(0, 0); writeStringLCD("irobot_linie"); mSleep(1500); clearLCD(); accuspannung(); clearLCD(); setMotorDir(FWD,FWD); speed = 80; kp = 1; //3 ki = 6; //10 kd = 40; //70 // Regler Parameter kd enthält bereits Division durch dt drest=0; isum=0; xalt=0; while(1) { FollowLine(); } return 0; }
die eigentliche "asuro-testarena" schafft der RP6 (zumindest mit diesem code) nicht, er sucht sich seinen eigenen weg, bricht öfters aus, findet aber immer wieder zu linie zurück...
Lesezeichen