viernes, 26 de octubre de 2012

C Code- Draft!

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