PARA UNA MEJOR LECTURA DEL CÓDIGO, COPIAR Y PEGAR EN LA VENTANA DE ARDUINO!
#include <APM_RC.h> // ArduPilot Mega RC Library #include <FastSerial.h> #include <SPI.h> #include <Arduino_Mega_ISR_Registry.h> #include <AP_PeriodicProcess.h> #include <AP_InertialSensor.h> #include <AP_IMU.h> #include <AP_ADC.h> #include <AP_Math.h> #include <AP_Common.h> #include <DataFlash.h> #include <AP_Compass.h> // Compass Library #include <I2C.h> //para dataflash #define HEAD_BYTE1 0xA3 #define HEAD_BYTE2 0x95 #define END_BYTE1 0xA2 #define END_BYTE2 0x4E #define DF_SLAVESELECT 53 #define ToRad(x) (x*0.01745329252) // *pi/180 #define ToDeg(x) (x*57.2957795131) // *180/pi #define A_LED_PIN 35 #define C_LED_PIN 37 #define LED_ON LOW #define LED_OFF HIGH #define MIN 1235 //evitamos que los motores se paren #define MAX 1500 #define CORTE 1200 Vector3f accel; //viene de la libreria math.h y es un vector 3D Vector3f gyro; FastSerialPort(Serial, 0); AP_Compass_HMC5843 compass; Arduino_Mega_ISR_Registry isr_registry; AP_TimerProcess adc_scheduler; APM_RC_APM1 APM_RC; DataFlash_APM1 DataFlash; /////////////////////////////////////////////////////////////////////////////////////////////////////////// //processing //int velocidadX=0,velocidadY=0; //int anguloX=0,anguloY=0; //int T,W,X,Y; //variables MOTORES int valor1=0; float throttle=MIN; float valor[7],valorF[7]; float pitch,roll; //Serial int aux=0,aux2=0; byte recived; boolean estado=false,datos=true; int8_t m_opp[7]={0,2,1,4,3,6,5}; ////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// //CONTROLADO1 float KpP=50,KdP=400,KiP=0.1; //PITCH //50 400 0.1 float KpR=50,KdR=300,KiR=0.1; //Roll //60 300 0.1 //PITCH float pitchSum=0,lastPitch=0,lastPitchFilter=0,pitchFilter=0; float giroPitch=0,lastGiroPitch =0,giroPitchFilter=0,lastGiroPitchFilter=0; //Roll float rollSum=0,lastRoll=0,lastRollFilter=0,rollFilter=0; float giroRoll=0,lastGiroRoll=0,giroRollFilter=0,lastGiroRollFilter=0,giroRoll2=0; //giroRoll lo usamos para hacer la derivación numérica float errorPitch=0,errorRoll=0; //YAW float compas=0; int kyaw=5; int yawi=0; int counter=0; //DATALOG long timer=0,timer1=0; //INPUTs float xref=0,yref=0,yaw=0,yawref=0; ///////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////// AP_ADC_ADS7844 adc; AP_InertialSensor_Oilpan oilpan_ins(&adc); AP_IMU_INS imu(&oilpan_ins,0); static void flash_leds(bool on) { digitalWrite(A_LED_PIN, on?LED_OFF:LED_ON); digitalWrite(C_LED_PIN, on?LED_ON:LED_OFF); } ///////////////////// T I M E R S static uint32_t fastTimer; static uint32_t fast_loopTimer; static byte medium_loopCounter; static uint32_t fiftyhz_loopTimer; static byte slow_loopCounter; static int16_t superslow_loopCounter; static byte counter_one_herz; //////////////////// void setup(void) { Serial.begin(115200); menu(); //imprime por menu las opciones para leer o borrar la memoria del log I2c.begin(); I2c.timeOut(20); if (!compass.init()) { Serial.println("compass initialisation failed!"); while (1) ; } isr_registry.init(); adc_scheduler.init(&isr_registry); APM_RC.Init(&isr_registry); // APM Radio initialization APM_RC.enable_out(CH_1); APM_RC.enable_out(CH_2); APM_RC.enable_out(CH_3); APM_RC.enable_out(CH_4); APM_RC.enable_out(CH_5); APM_RC.enable_out(CH_6); APM_RC.enable_out(CH_7); APM_RC.enable_out(CH_8); APM_RC.OutputCh(1, valor1); APM_RC.OutputCh(2, valor1); APM_RC.OutputCh(3, valor1); APM_RC.OutputCh(4, valor1); APM_RC.OutputCh(5, valor1); APM_RC.OutputCh(6, valor1); DataFlash.Init(); DataFlash.StartWrite(1); //empezamos a escribir en la página nº1 /* Should also call ins.init and adc.init */ imu.init(IMU::COLD_START, delay, flash_leds, &adc_scheduler); imu.init_accel(delay, flash_leds);Serial.print("\n"); funcion_serial(); fast_loopTimer=micros();timer1=micros(); compass.set_orientation(AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD); // set compass's orientation on aircraft. compass.set_offsets(0,0,0); // set offsets to account for surrounding interference compass.set_declination(ToRad(0.23)); // set local difference between magnetic north and true north para cartagena/murcia la declinación es 0.28 } void loop(void) { int32_t timer = micros(); if ((timer - fast_loopTimer) >= 10000) { //400 Hz fast_loop(); fast_loopTimer=micros(); } if ((timer - fiftyhz_loopTimer) >= 20000) { // reads all of the necessary trig functions for cameras, throttle, etc. fiftyhz_loopTimer = timer; medium_loop(); //10 Hz fiftyhz_loop(); //50 Hz counter_one_herz++; if(counter_one_herz == 50){ super_slow_loop(); //1 Hz counter_one_herz = 0; } } }//cierra el void loop // F U N C I O N E S static void fast_loop() { if(estado){ imu.update(); accel = imu.get_accel(); gyro = imu.get_gyro(); pitch = accel.y; roll = accel.x; giroPitch= gyro.x; giroRoll = gyro.y; //FILTER pitch pitchFilter=lastPitchFilter*0.9048 + 0.09516*lastPitch; //giroRoll2 = ((pitchFilter-lastPitchFilter)*100000)/(micros()-timer1); timer1=micros(); derivacion numérica // giroPitchFilter=lastGiroPitchFilter*0.6703 + 0.3297 *lastGiroPitch; //filter Roll rollFilter=lastRollFilter*0.9048 + 0.09516*lastRoll; // giroRollFilter=lastGiroRollFilter*0.07318 + 0.9268 *lastGiroRoll; //filtro t=0.0038, G=tf([1],[0.05 1]);c2d(G,0.0038) ensayo d9_10 demasiado suave // giroRollFilter=lastGiroRollFilter*0.141 + 0.859 *lastGiroRoll; // G=tf([1],[0.025 1]);c2d(G,0.0038 giroRollFilter=lastGiroRollFilter*0.00253 + 0.9975 *lastGiroRoll; //G=tf([1],[1.5 1]);c2d(G,0.0038) parece que funciona //CUando no tenemos filtro // rollFilter = roll; //si activamos o desactivamos los filtros, hay que activar el actualizar los valores abajo de la funcion // pitchFilter = pitch; giroPitchFilter=giroPitch; pitchSum +=errorPitch; //integral //giroRollFilter=giroRoll; rollSum += errorRoll; //integral errorPitch=yref-pitchFilter; errorRoll=xref-rollFilter; //FRONT RED valor[1] = throttle + 1.0* (KpR*errorRoll - KdR*giroRollFilter + KiR*rollSum); valor[5] = throttle + 0.5* (KpR*errorRoll - KdR*giroRollFilter + KiR*rollSum) -0.866* (KpP*errorPitch +KdP*giroPitchFilter + KiP*pitchSum); valor[4] = throttle + 0.5* (KpR*errorRoll - KdR*giroRollFilter + KiR*rollSum) +0.866* (KpP*errorPitch +KdP*giroPitchFilter + KiP*pitchSum); //BACK BLACK valor[2] = throttle - 1.0* (KpR*errorRoll - KdR*giroRollFilter + KiR*rollSum); valor[3] = throttle - 0.5* (KpR*errorRoll - KdR*giroRollFilter + KiR*rollSum) -0.866* (KpP*errorPitch +KdP*giroPitchFilter + KiP*pitchSum); valor[6] = throttle - 0.5* (KpR*errorRoll - KdR*giroRollFilter + KiR*rollSum) +0.866* (KpP*errorPitch +KdP*giroPitchFilter + KiP*pitchSum); // CONTROL DEL YAW /* yaw=compas-yawref; yawi=+yaw; valor[1] +=kyaw*yaw +yawi; valor[3] +=kyaw*yaw +yawi; valor[6] +=kyaw*yaw +yawi; valor[2] -=kyaw*yaw -yawi; valor[4] -=kyaw*yaw -yawi; valor[5] -=kyaw*yaw -yawi;*/ ////////////////////////////////////////////////////// escribe_data(); ////// for(int8_t m=1;m<=6;m++){ //TRIDGE STABILITY PATCH pag 49 hexamotors. Recalcula las velocidades si alguna de ellas satura. if(valor[m]>MAX){ valor[m_opp[m]]-=valor[m]-MAX; valor[m]=MAX; }} for(int8_t i=1;i<=6;i++){ //CONSTRAIN --> limites superior e inferior valor[i] = constrain(valor[i],MIN,MAX); } /* //FILTRO que frena la acelearcion contra la deceleracion for(int8_t m=1;m<=6;m++){ if(valorF[m]<valorF[m]){ //es porq estamos acelerando valorF[m]=(valor[m]+valorF[m])/2; } else{ //no filtramos valorF[m]=valor[m]; }} */ /* if(counter=100){ pitchSum=0; //reseteamos integral en un determinado tiempo // rollSum=0; counter=0; } counter++; */ actualiza_motores(); //actualizamos los valores pitch para el filtro lastPitch=pitch; // lastGiroPitch=giroPitch; lastPitchFilter=pitchFilter; //lastGiroPitchFilter=giroPitchFilter; lastRoll=roll; lastGiroRoll=giroRoll; lastRollFilter=rollFilter; lastGiroRollFilter=giroRollFilter; }//cerramos if(estado) else{ funcion_serial(); //Si el throttle está a cero --> imprimimos VELOCIDAD CERO a los motores! y calibramos el PID for(int8_t i=1;i<=6;i++){ APM_RC.OutputCh(i, 0); } } /* if(estado){ //imprime datos por Serial.Monitor Serial.printf(" %4.4f , %4.4f , %4.4f , %4.4f , %4.4f , %4.4f ,", accel.x, accel.y, rollFilter, gyro.x, gyro.y, pitchFilter);Serial.print(millis()); Serial.print(",");Serial.print(valor[1]); Serial.print(",");Serial.print(valor[2]); Serial.print(",");Serial.print(valor[3]); Serial.print(",");Serial.print(valor[4]); Serial.print(",");Serial.print(valor[5]); Serial.print(",");Serial.print(valor[6]); Serial.print(",");Serial.print(xref); Serial.print(",");Serial.print(yref); Serial.print(",");Serial.print(throttle); Serial.print(",");Serial.println(yaw); }*/ } //cierra fast_loop static void medium_loop() { switch(medium_loopCounter) { // Tenemos en cuenta el Compass //----------------------------------------- case 0: medium_loopCounter++; compass.read(); compass.calculate(0,0); compas=ToDeg(compass.heading); Serial.print(compas);Serial.print(", ");Serial.print(yawref);Serial.print(", ");Serial.println(yaw); break; // Leemos valores de las referencias //------------------------------------------------ case 1: medium_loopCounter++; yref=(APM_RC.InputCh(CH_2)-1050);yref/=210;yref-=2;yref=constrain(yref,-1,1); //pitch //xref=(APM_RC.InputCh(CH_2)-1050);xref/=210;xref-=2;constrain(xref,-0.5,0.5); //roll //yaw=APM_RC.InputCh(CH_4)-1050;yaw/=100;yaw-=4; break; // command processing //------------------- case 2: medium_loopCounter++; break; // This case deals with sending high rate telemetry //------------------------------------------------- case 3: medium_loopCounter++; break; // This case controls the slow loop //--------------------------------- case 4: medium_loopCounter = 0; break; default: // this is just a catch all // ------------------------ medium_loopCounter = 0; break; } }//fin medium_loop static void fiftyhz_loop(){ //leer datos del acelerador if(APM_RC.InputCh(CH_3)>CORTE){ // throttle = APM_RC.InputCh(CH_3); //acelerador variable throttle=1280; //acelerador constante estado=true; // escribe_data(); //RECIBIMOS DATOS CADA 50HZ (20ms() }else{estado=false;timer=millis();} } static void super_slow_loop() //loop de un Herz { } void actualiza_motores() { for(int8_t i=1;i<=6;i++){ //el throttle está funcionando --> IMPRIMIR las VELOCIDADES a los motores APM_RC.OutputCh(i,valor[i]); } } static void escribe_data() { DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteInt(rollFilter*10000); //rollFIlter DataFlash.WriteInt(pitchFilter*10000); //pitch filter DataFlash.WriteInt(giroRollFilter*10000); //gyroRoll DataFlash.WriteInt(gyro.y*10000); //gyroPitch DataFlash.WriteLong((long)(millis()-timer)); //time DataFlash.WriteInt(valor[1]); //velocidades motores DataFlash.WriteInt(valor[2]); //motor 1 y 2 para el roll DataFlash.WriteInt(valor[4]); //motor 4 y 5 para el pitch DataFlash.WriteInt(valor[5]); DataFlash.WriteInt(xref*100); //x ref DataFlash.WriteInt(yref*100); //yref DataFlash.WriteInt(compas*100); DataFlash.WriteInt(throttle); //throttle DataFlash.WriteByte(END_BYTE1); // 2 bytes of checksum (example) DataFlash.WriteByte(END_BYTE2); } /*static void escribe_espacio() { DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteInt(); //rollFIlter DataFlash.WriteInt(pitchFilter*10000); //pitch filter DataFlash.WriteInt(gyro.y*10000); //gyroRoll DataFlash.WriteInt(giroRollFilter*10000); //gyroPitch DataFlash.WriteLong((long)(millis()-timer)); //time DataFlash.WriteInt(valor[1]); //velocidades motores DataFlash.WriteInt(valor[2]); //motor 1 y 2 para el roll DataFlash.WriteInt(valor[4]); //motor 4 y 5 para el pitch DataFlash.WriteInt(valor[5]); DataFlash.WriteInt(counter); //y ref DataFlash.WriteInt(throttle); //throttle DataFlash.WriteByte(END_BYTE1); // 2 bytes of checksum (example) DataFlash.WriteByte(END_BYTE2); counter=0; }*/ static void funcion_serial() { if(Serial.available()>0){ byte recived=Serial.read(); if(recived==76||recived==108){ // L OR l //LEER int i, tmp_int; byte tmp_byte1, tmp_byte2; long tmp_long; Serial.println("empezando a leer..."); DataFlash.StartRead(1); // We start reading from page 1 for(int i;i<10000;i++){ // Read 1000 packets... tmp_byte1 = DataFlash.ReadByte(); tmp_byte2 = DataFlash.ReadByte(); if ((tmp_byte1 == HEAD_BYTE1) && (tmp_byte1 == HEAD_BYTE1)){ // Read 4 ints... tmp_int = DataFlash.ReadInt(); //*10 000 if(isSpace(tmp_int)) { tmp_int = DataFlash.ReadInt(); if(isSpace(tmp_int)) { Serial.println("espacio encontrado"); break; }} Serial.print(tmp_int); Serial.print(","); tmp_int = DataFlash.ReadInt(); Serial.print(tmp_int); Serial.print(","); tmp_int = DataFlash.ReadInt(); Serial.print(tmp_int); Serial.print(","); tmp_int = DataFlash.ReadInt(); Serial.print(tmp_int); Serial.print(","); // Read 2 longs... tmp_long = DataFlash.ReadLong(); Serial.print(tmp_long); Serial.print(","); // Read 6 ints... tmp_int = DataFlash.ReadInt(); //motores Serial.print(tmp_int); Serial.print(","); tmp_int = DataFlash.ReadInt(); Serial.print(tmp_int); Serial.print(","); tmp_int = DataFlash.ReadInt(); Serial.print(tmp_int); Serial.print(","); tmp_int = DataFlash.ReadInt(); Serial.print(tmp_int); Serial.print(","); tmp_int = DataFlash.ReadInt(); //xref *100 Serial.print(tmp_int); Serial.print(","); tmp_int = DataFlash.ReadInt(); //yref *100 Serial.print(tmp_int); Serial.print(","); tmp_int = DataFlash.ReadInt(); //COMPAS *100 Serial.print(tmp_int); Serial.print(","); tmp_int = DataFlash.ReadInt(); //throttle Serial.print(tmp_int); // Read the checksum... tmp_byte1 = DataFlash.ReadByte(); tmp_byte2 = DataFlash.ReadByte(); } Serial.println(); } Serial.println(""); Serial.println("lectura completada "); Serial.println(""); while(1); } else if(recived==98||recived==66){ //B OR b //DELETE borrar(); while(1); } } } void dataflash_CS_inactivee() { digitalWrite(DF_SLAVESELECT,HIGH); //disable device } void dataflash_CS_activee() { digitalWrite(DF_SLAVESELECT,LOW); //enable device } static void borrar() { dataflash_CS_activee(); SPI.transfer(0xC7); SPI.transfer(0x94); SPI.transfer(0x80); SPI.transfer(0x9A); dataflash_CS_inactivee(); Serial.println("Borrado de memoria Flash completado"); } static void menu() { Serial.println("Bienvenido;\n \t\t\t pulse.... \n 'L' para Leer datos \n 'B' para Borrar datos\n \t\t\t\t\t y despues INTRO"); }
No hay comentarios:
Publicar un comentario