Show Posts
Pages: 1 ... 4 5 [6] 7 8 9
76  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 27, 2013, 11:04:45 am
4454.1740, 09325.0143, -030.7 not a float because of the decimal point? i was under the impression arduino stored numbers as integers?
77  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 27, 2013, 10:36:57 am
more to the point i asked my self if i WANT that much precision and i found i told my self to kiss off. smiley-twist that said xx.xx is juuuust fine. now i just need to transmit this heap of wholesome goodness
Code:
$GPRMC,140053.00,A,4454.1740,N,09325.0143,W,000.0,128.7,300508,001.1,E,A*2E
$GPGGA,140053.00,4454.1740,N,09325.0143,W,1,09,01.1,00289.8,M,-030.7,M,,*5E
$GPGSA,A,3,21,15,18,24,26,29,06,22,,03,,,02.0,01.1,01.7*04
$GPGSV,3,1,12,21,75,306,40,15,59,075,46,18,57,269,49,24,56,115,46*79
$GPGSV,3,2,12,26,48,059,43,29,27,188,48,06,25,308,41,22,18,257,33*7D
$GPGSV,3,3,12,08,14,060,,03,11,320,32,09,06,144,,16,04,311,*7C
$GPRMC,140054.00,A,4454.1740,N,09325.0143,W,000.0,128.7,300508,001.1,E,A*29
$GPGGA,140054.00,4454.1740,N,09325.0143,W,1,09,01.1,00289.8,M,-030.7,M,,*59
$GPGSA,A,3,21,15,18,24,26,29,06,22,,03,,,02.0,01.1,01.7*04
$GPGSV,3,1,12,21,75,306,40,15,59,075,46,18,57,269,49,24,56,115,46*79
$GPGSV,3,2,12,26,48,059,43,29,27,188,48,06,25,308,41,22,18,257,33*7D
$GPGSV,3,3,12,08,14,060,,03,11,320,32,09,06,144,,16,04,311,*7C

Hmmm let's see if me missed anything.. floats/doubles check, chars and ints yup got plenty of those and most of shift + 1-9 yup got me some of those in spades. yeah there so has to be a better way of cramming that into the NRF FIFO and sucking it back out on the other end. please tell me there is PaulS please? smiley-eek
78  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 27, 2013, 06:14:22 am
now why cant i get more than xx.xx is it because of the float > byte > float conversion?
79  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 27, 2013, 06:12:33 am
Mirf.payload = 4 * sizeof(byte); needed to be 12  Mirf.payload = 12 * sizeof(byte); on both and BOOM!. aint she perdy!
pointing the bot a lil NE and more or less flat
Code:
0.47, -0.30, -3.59
0.47, -0.30, -3.59
0.47, -0.30, -3.59
0.47, -0.30, -3.59
south and flat
Code:
0.69, 0.22, -119.77
0.69, 0.22, -119.77
0.69, 0.22, -119.77
0.69, 0.22, -119.77
tilted back 90 degrees and facing south.
Code:
87.48, 0.42, -104.21
87.48, 0.42, -104.21
87.48, 0.42, -104.21
87.48, 0.42, -104.21
87.48, 0.42, -104.21
on it side facing south
Code:
-15.87, -89.33, -122.80
-15.87, -89.33, -122.80
-15.87, -89.33, -122.80
-15.87, -89.33, -122.80
-15.87, -89.33, -122.80
-15.87, -89.33, -122.80
-15.87, -89.33, -122.80

looks like i got it! yes i am very excited.  smiley-lol smiley-cool smiley-lol
80  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 27, 2013, 05:22:58 am
Well so far so good maybe? it's late agin  5:18am here need some sleeps got 2 hours on yesterday

TX
Code:
float Tflo2byte12(float inRoll, float inPitch, float inYaw){

  union inRoll2byte_t {
    byte  inRollout[4];
    float inRollin;
  }
  inRoll2byte;
  inRoll2byte.inRollin = inRoll;

  union inPitch2byte_t {
    byte  inPitchout[4];
    float inPitchin;
  }
  inPitch2byte;
  inPitch2byte.inPitchin = inPitch;
 
  union inYaw2byte_t {
    byte  inYawout[4];
    float inYawin;
  }
  inYaw2byte;
  inYaw2byte.inYawin = inYaw;
 
  byte output[12];
  output[0] = inRoll2byte.inRollout[0];
  output[1] = inRoll2byte.inRollout[1]; 
  output[2] = inRoll2byte.inRollout[2];
  output[3] = inRoll2byte.inRollout[3];
 
 output[4] = inPitch2byte.inPitchout[0];
  output[5] = inPitch2byte.inPitchout[1]; 
  output[6] = inPitch2byte.inPitchout[2];
  output[7] = inPitch2byte.inPitchout[3];

  output[8] = inYaw2byte.inYawout[0];
  output[9] = inYaw2byte.inYawout[1]; 
  output[10] = inYaw2byte.inYawout[2];
  output[11] = inYaw2byte.inYawout[3];


  Mirf.setTADDR((byte *)"base1"); // set name of Reciever
  Mirf.send(output); // data to send
  while(Mirf.isSending()){
  }
  delay(10);
}

RX above setup
Code:

union inRollrf_t{
  byte  incomRoll[4];
  float imuRoll;
}
inRollrf;

union inPitchrf_t{
  byte  incomPitch[4];
  float imuPitch;
}
inPitchrf;

union inYawrf_t{
  byte  incomYaw[4];
  float imuYaw;
}
inYawrf;
Loop
Code:
void loop(){
  //-+-+-+>>>>Start data collection from transciever<<<<<<-+-+-+
  byte data[Mirf.payload];
  if(Mirf.dataReady()){
    do{
      Mirf.getData(data);
      inB2F(data); // send to byte to float converter
      delay(10);
    }
    while(!Mirf.rxFifoEmpty());
  }
    Serial.print(inRollrf.imuRoll);
    Serial.print(", ");
    Serial.print(inPitchrf.imuPitch);
    Serial.print(", ");
    Serial.print(inYawrf.imuYaw);
    Serial.println("");
} // end of program

external called form loop
Code:
void inB2F(byte data[]){
  //*********** Start convertion from byte to float *********
  inRollrf.incomRoll[0] = data[0];
  inRollrf.incomRoll[1] = data[1];
  inRollrf.incomRoll[2] = data[2];
  inRollrf.incomRoll[3] = data[3];
 
  inPitchrf.incomPitch[0] = data[4];
  inPitchrf.incomPitch[1] = data[5];
  inPitchrf.incomPitch[2] = data[6];
  inPitchrf.incomPitch[3] = data[7];
 
  inYawrf.incomYaw[0] = data[8];
  inYawrf.incomYaw[1] = data[9];
  inYawrf.incomYaw[2] = data[10];
  inYawrf.incomYaw[3] = data[11];


think i am getting closer.  formatted text is nice but only first field is filling? -0.15, 0.00, 0.00
any guidance would be great i am so far out in uncharted territory for me is unreal. and why am i only getting xx.xx and not xx.xxx ? yes that amount of precision would be handy.
81  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 26, 2013, 10:30:41 pm
welp heading to bed  smiley-sleep, maybe it will come to me in the morning. smiley-eek-blue
82  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 26, 2013, 06:18:03 pm
looks like there getting mucked with before i send them im assuming from making a byte array out of them. so question how do i get the floats out of the struct in a way the NRF will accept them or am i missing something with Mirf and the NRF24 it self that is causing me to have to use bytes and not send floats.
83  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 26, 2013, 03:37:05 pm
Ok so far here is the transmit seems to be working

Code:

    typedef struct
    {
    float roll; // place roll value here
    float pitch;// place pitch value here
    float yaw;  // place yaw value here
   }RPYT_type; // lable the struct

  RPYT_type imuVal = {inRoll,inPitch,inYaw}; // give float roll in RPYT_type value from incoming function argumet inRoll
  Serial.println("");
    Serial.print(imuVal.roll);
  Serial.print(" , ");
    Serial.print(imuVal.pitch);
  Serial.print(" , ");
    Serial.print(imuVal.yaw);
  Serial.println("");
  byte vals[3];
  vals[0] = imuVal.roll;
  vals[1] = imuVal.pitch; 
  vals[2] = imuVal.yaw;
  Mirf.setTADDR((byte *)"base1"); // set name of Reciever
  Mirf.send(vals); // figure out how to access and transmit second array segment containing inRoll,inPitch,inYaw

but on my rec side i have
Code:
void RPYT(byte data[3]){
  //*********** Start convertion from byte to float *********
  typedef struct{
    float roll;
    float pitch;
    float yaw;
   } RPYT_type;
  RPYT_type imuVal = {data[0],data[1],data[2]};
 
  Serial.print(imuVal.roll);
  Serial.print("'");
  Serial.print(imuVal.pitch);
  Serial.print("'");
  Serial.print(imuVal.yaw);
  Serial.println("");

my TX is sending this
Code:
-0.00 , -0.01 , 0.01
3.23 , -0.33 , 0.10
2.43 , 0.01 , 0.09
1.95 , -0.39 , 0.08
0.64 , 0.37 , 0.07
RX is displaying this
Code:
0.00, 0.00, 0.00
3.00, 0.00, 0.00
2.00, 0.00, 0.00
1.00, 0.00, 0.00
0.00, 0.00, 0.00

i know im tired but i am seriously stumped
84  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 26, 2013, 03:04:15 pm
ahh ok think im starting to see now

Code:
void RPYtransmit(float inRoll, float inPitch, float inYaw){ //recieve current vals for ToDeg(roll),ToDeg(pitch),ToDeg(yaw)
 
    typedef struct
    {
    float roll; // place roll value here
    float pitch;// place pitch value here
    float yaw;  // place yaw value here
   }RPYT_type; // lable the struct

  RPYT_type imuVal = {inRoll,inPitch,inYaw}; // give float roll in RPYT_type value from incoming function argumet inRoll
  Serial.println("");
    Serial.print(imuVal.roll);
  Serial.print(" , ");
    Serial.print(imuVal.pitch);
  Serial.print(" , ");
    Serial.print(imuVal.yaw);
  Serial.println("");
}
so how do i trick Mirf.send(); into thinking this is once piece of data?
85  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 26, 2013, 01:57:34 pm
eep! 2? my understanding of stucts must be way off then here is what i understood it to be
Code:
RPYtransmit(ToDeg(roll),ToDeg(pitch),ToDeg(yaw));
Code:
void RPYtransmit(float inRoll, float inPitch, float inYaw){ //recieve current vals for ToDeg(roll),ToDeg(pitch),ToDeg(yaw)
 
    typedef struct
    {
    float roll; // place for roll value here
    float pitch;// place for pitch value here
    float yaw;  // place for yaw value here
   }RPYT_type; // lable the struct

    RPYT_type imuArray[2]; // create an array to hold roll,pitch,yaw for transmission


  imuArray[0].roll  = inRoll; // give float roll in RPYT_type value from incoming function argument inRoll
  imuArray[0].pitch = inPitch;// give float pitch in RPYT_type value from incoming function argument inPitch
  imuArray[0].yaw   = inYaw;// give float yaw in RPYT_type value from incoming function argument inYaw
  imuArray[1] = (RPYT_type) {inRoll,inPitch,inYaw}; // combine inRoll,inPitch,inYaw from RPYT_type into imuArray position 2 (0 being first position in array)

  Mirf.setTADDR((byte *)"base1"); // set name of Receiver
  Mirf.send(imuArray[2]); // figure out how to access and transmit second array segment containing inRoll,inPitch,inYaw
  while(Mirf.isSending()){
  }
  delay(10);
}
86  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 26, 2013, 12:45:54 pm
The function to send an array of bytes can send the bytes from any memory location. Define a struct. Store the data in an instance of the struct. Call the function to send the data with the struct instance and the size of the struct.

That function does not want a struct? Lie to it. Tell it that the struct is really a collection of bytes, using a cast - (byte *). Use the sizeof() function to get the number of bytes to send.

this does not compile i know. just wanted to know if i am heading in the right direction
Code:
   typedef struct{
    float roll;
    float pitch;
    float yaw;
  }floatImu;
 
   floatImu inImu[2];
  inImu[0].roll  = ToDeg(roll);
  inImu[0].pitch = ToDeg(pitch);
  inImu[0].yaw   = ToDeg(yaw);
  inImu[1] = (floatImu){roll,pitch,yaw};
  outD = inImu[1];
 
  Mirf.setTADDR((byte *)"base1"); // set name of Reciever
  Mirf.send(outD); // data to send
  while(Mirf.isSending()){
  }
  delay(10);
87  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 26, 2013, 11:20:20 am
like this but with structs?
i did this last night before i went to bed.
TX
Code:
float Tflo2byte(float inflo){

  union flow2byte_t {
    byte  outbyte[4];
    float infloat;
  }
  flow2byte;
  flow2byte.infloat = inflo;
  byte output[4];
  output[0] = flow2byte.outbyte[0];
  output[1] = flow2byte.outbyte[1]; 
  output[2] = flow2byte.outbyte[2];
  output[3] = flow2byte.outbyte[3];

  Mirf.setTADDR((byte *)"base1"); // set name of Reciever
  Mirf.send(output); // data to send
  while(Mirf.isSending()){
  }
  delay(10);
}

Rx side in main loop
Code:
byte data[Mirf.payload];
  if(Mirf.dataReady()){
    do{
      Mirf.getData(data);
      byte2flo(data); // send to byte to float converter
      delay(10);
    }
    while(!Mirf.rxFifoEmpty());
  }
external to loop
Code:
void byte2flo(byte data[]){
  //*********** Start convertion from byte to float *********
  union byte2flo_t {
    byte  outbyte[4];
    float outfloat;
  }
  byte2flo;
  byte2flo.outbyte[0]= data[0];
  byte2flo.outbyte[1]= data[1]; 
  byte2flo.outbyte[2]= data[2];
  byte2flo.outbyte[3]= data[3];
  Serial.println(byte2flo.outfloat);
  //*********** END convertion from byte to float *********
 
}
works but this is the output
 0.38
-1.28
-12.58
now i have to "collect" the data and store it so i can turn that into something along the lines !IMU:0.38,-1.28,-12.58: (!) start of line (smiley end of line for external programs like processing or python.
point me in the general direction i need to do this? like some kind of scanning routine that watches the incoming data or something.
ill go check out structs now. thanks for the input PaulS and michinyon
i appreciate this a lot.
88  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 26, 2013, 01:39:31 am
WOW thanks i got your post a couple min after to made and and just got it to work. here is what i did

TX side
Code:
union imur_t {
   byte  Uroll[4];
   float froll;
 }imur;
   imur.froll = ToDeg(roll);
  byte vals[4];
  vals[0] = imur.Uroll[0];
  vals[1] = imur.Uroll[1]; 
  vals[2] = imur.Uroll[2];
  vals[3] = imur.Uroll[3];
then i push vals out like before in the Nrf function
and i did the reverse on the other side like this

RX side
Code:
union imur_t {
   byte  Uroll[4];
   float froll;
 }imur;
    imur.Uroll[0] = data[0];
    imur.Uroll[1] = data[1];
    imur.Uroll[2] = data[2];
    imur.Uroll[3] = data[3];
and then printed it out and boom there it was. -0.03 or 1.37 etc what ever the roll on the IMU was it sent it across. WOOT!. yes i am excited cos that %100 my own work. took over  WELL over 100 tries the past few hours. it's now 1:33am here time for bedZ. now the big question is there better why than this because i have roll pitch yaw just on the imu and then i have the GPS sentences and the ultrasonic stuffs to do along with others. do i have to create... ( brain fart, cant i just make a function and send it transmit var in the type of a float and have it return the bytes and send them auto to the nrf?) like f2bconv(Sensor1()); and sen 1 being in form of a float be chopped and passed to Mirf.send? sorry it is real  late here and i am not "good" at this fresh in the morning much less now lol. ill translate this in the morning! smiley ( i hope)
89  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 25, 2013, 08:20:18 pm
i use the ESC codes for VT100 term output, i took them out because i know most of you use the generic arduino ide serial monitor.
They now have been removed.
as for the return well i am still quite learning i just learned last week how to pass arguments to external functions so still need lots of work learning how to get them out.
i am still shopping in the noob section i think.
as for trying to stuff floats into byte array yea i know it would not work but it showed what i need to accomplish.
 if i had a nickle for every float to byte, f to b array converter i have looked at in the past 4 days i would be able to buy a starbucks coffee.
i am thinking type casting is where i need to look? or i have seen some typedef union stuff.
so to answer your question NO i do not know much about var types and i am not that experienced with programing on any level.
but i keep plugging away at it and learning as i go.
so i will say sorry now for being bad at programming and if that is not welcome here let me know.
so any ideas on fixes for the return flat,flon and ANY help on the float to byte method would be fantastic.
90  Using Arduino / Programming Questions / Re: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 on: January 25, 2013, 04:55:49 pm
Part_02
Code:

void loop()
{
  unsigned  int distanceFwd = averagePing();
  if (distanceFwd > dangerThresh){
    MoveForward(Mspeed1,runs2,mDelay1);
    WiskerCheck(); 
  }
  else{
    PingScan(distanceFwd);
    WiskerCheck(); 
  }
 
  TgpsOut();
  MinImu9();
  NrfTrans();
  WiskerCheck(); 
}
//-+-+-+>>>> Start Nrf24L01 Transmitter Section<<<<<<-+-+-+
void NrfTrans(){
  //****************************Info being Sent**********************
  // Ultrasonic Vals (1 val)
  // GPS NMEA data (? vals)
  // IMU Roll Pitch Yaw (3 vals)
  // Wisker hits (2vals)
  //**************put values in array?**************
  byte vals[6];
  //**********Send IMU Roll Pitch Yaw values**********
  vals[0] =ToDeg(roll);
  vals[1] =ToDeg(pitch); 
  vals[2] =ToDeg(yaw);
  vals[3] =flat;
  vals[4] =flon;
  vals[5] =Tgps.satellites();
  //**************************************************
  //**************finish puting values in array?**************
  //**************Start Transmit package**************
  Mirf.setTADDR((byte *)"base1"); // set name of Reciever
  Mirf.send(vals); // data to send
  Serial.println(" Sent");
  while(Mirf.isSending()){
  }
  delay(10);
}
//-+-+-+>>>> End Nrf24L01 Transmitter Section<<<<<<-+-+-+
float TgpsOut()
{
  bool newData = false;
  unsigned long chars;
  unsigned short sentences, failed;
  // For one second we parse GPS data and report some key values
  for (unsigned long start = millis(); millis() - start < 100;)
  {
    while (Serial2.available())
    {
      char c = Serial2.read();
      // Serial.write(c); // uncomment this line if you want to see the GPS data flowing
      if (Tgps.encode(c)) // Did a new valid sentence come in?
        newData = true;
    }
  }

  if (newData)
  {
    //    float flat, flon;
    unsigned long age;
    Tgps.f_get_position(&flat, &flon, &age);
    /*
    Serial.print("LAT=");
     Serial.print(flat == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flat, 6);
     Serial.print(" LON=");
     Serial.print(flon == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flon, 6);
     Serial.print(" SAT=");
     Serial.print(Tgps.satellites() == TinyGPS::GPS_INVALID_SATELLITES ? 0 : Tgps.satellites());
     Serial.print(" PREC=");
     Serial.print(Tgps.hdop() == TinyGPS::GPS_INVALID_HDOP ? 0 : Tgps.hdop());
     */
    return flat,flon;     
  }
  /*
  Tgps.stats(&chars, &sentences, &failed);
   Serial.print(" CHARS=");
   Serial.print(chars);
   Serial.print(" SENTENCES=");
   Serial.print(sentences);
   Serial.print(" CSUM ERR=");
   Serial.println(failed);
   */
}
//-+-+-+>>>> START Sensor readings section<<<<<<-+-+-+
unsigned int  ping(){
  WiskerCheck();
  delay(50);                      // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
  return uS / US_ROUNDTRIP_CM;
}
unsigned int averagePing(){
  total= total - readings[index]; // subtract the last reading:
  readings[index] = ping();   // read from the sensor: 
  total= total + readings[index];   // add the reading to the total:
  index = index + 1;  // advance to the next position in the array: 
  if (index >= numReadings)  // if we're at the end of the array...
    index = 0;   // ...wrap around to the beginning:                           
  average = total / numReadings;   // calculate the average:
  delay(1);        // delay in between reads for stability           
  return average;   // send it as ASCII digits
}
unsigned int PingScan(unsigned int Fwddist){
  if (Fwddist <= dangerThresh){
    centerDistance  = averagePing();
    if (centerDistance <= dangerThresh){
      MoveBackward(Mspeed2,runs2,mDelay2);
      MoveLeft(Mspeed1,runs1,mDelay1);
      WiskerCheck();
    }
    else{
      return 0;
    }

  }

void WiskerCheck(){
  int Tcount = 5;
  oldLeftWiskerValue = leftWiskerValue;
  oldRightWiskerValue = rightWiskerValue;
  leftWiskerValue = digitalRead(leftWiskerPin);
  rightWiskerValue = digitalRead(rightWiskerPin); 
  if((leftWiskerValue == LOW) && (oldLeftWiskerValue == HIGH))
  {
    MoveStop(Mspeed0,runs1,mDelay1);
    for (int LwiskT =0; LwiskT < Tcount; LwiskT++){
      MoveBackward(Mspeed2,runs2,mDelay2);
      MoveRight(Mspeed2,runs2,mDelay2);
    }
    return;
  }
  if((rightWiskerValue == LOW) && (oldRightWiskerValue == HIGH))
  {
    MoveStop(Mspeed0,runs1,mDelay1);
    for (int LwiskT =0; LwiskT < Tcount; LwiskT++){
      MoveBackward(Mspeed2,runs2,mDelay2);
      MoveLeft(Mspeed2,runs2,mDelay2);
    }
    return;
  }
  else{
    //Serial.println("=>WiskerCheck: else triggered<=");
    return;
  }
  return;
}
//-+-+-+>>>>START POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
void MinImu9() //PoloLU MINIMU -9DOF
{

  if((millis()-timer)>=20)  // Main loop runs at 50Hz
  {
    counter++;
    timer_old = timer;
    timer=millis();
    if (timer>timer_old)
      G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
    else
      G_Dt = 0;
    // *** DCM algorithm
    // Data adquisition
    Read_Gyro();   // This read gyro data
    Read_Accel();     // Read I2C accelerometer
    if (counter > 5)  // Read compass data at 10Hz... (5 loop runs)
    {
      counter=0;
      Read_Compass();    // Read I2C magnetometer
      Compass_Heading(); // Calculate magnetic heading 
    }
    // Calculations...
    Matrix_update();
    Normalize();
    Drift_correction();
    Euler_angles();
    // ***
    //   VTprintdata();
    // printdata();
  }
}
//-+-+-+>>>> END POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
//-+-+-+>>>> END Sensor readings section<<<<<<-+-+-+

//-+-+-+>>>> Motor Driver section<<<<<<-+-+-+
void MoveLeft(int SPD, int mTime, int mDelay)
{
  //int oTime = 0;
  while (oTime < mTime){
    digitalWrite(ENA_A, LOW);
    digitalWrite(DIR_A, HIGH);
    analogWrite(PWM_A, SPD);
    digitalWrite(ENA_B, LOW);
    digitalWrite(DIR_B, LOW);
    analogWrite(PWM_B, SPD);
    delay(mDelay1);
    oTime++;
  }
}

void MoveRight(int SPD, int mTime, int mDelay)
{
  //  int oTime = 0;
  while (oTime < mTime){
    digitalWrite(ENA_A, LOW);
    digitalWrite(DIR_A, LOW);
    analogWrite(PWM_A, SPD);
    digitalWrite(ENA_B, LOW);
    digitalWrite(DIR_B, HIGH);
    analogWrite(PWM_B, SPD);
    delay(mDelay);
    oTime++;
  }
}
void MoveForward(int SPD, int mTime,int mDelay)
{
  //  int oTime = 0;
  while (oTime < mTime){
    digitalWrite(ENA_A, LOW);
    digitalWrite(DIR_A, HIGH);
    analogWrite(PWM_A, SPD);
    digitalWrite(ENA_B, LOW);
    digitalWrite(DIR_B, HIGH);
    analogWrite(PWM_B, SPD); //move forward
    delay(mDelay);
    oTime++;
  }
}
void MoveBackward(int SPD, int mTime,int mDelay)
{
  //  int oTime = 0;
  while (oTime < mTime){
    digitalWrite(ENA_A, LOW);
    digitalWrite(DIR_A, LOW);
    analogWrite(PWM_A, SPD);
    digitalWrite(ENA_B, LOW);
    digitalWrite(DIR_B, LOW);
    analogWrite(PWM_B, SPD);
    delay(mDelay);
    oTime++;
  }
}
void MoveStop(int SPD, int mTime,int mDelay)
{
  //  int oTime = 0;
  while (oTime < mTime){
    digitalWrite(ENA_A, HIGH);
    digitalWrite(DIR_A, HIGH);
    analogWrite(PWM_A, SPD);
    digitalWrite(ENA_B, HIGH);
    digitalWrite(DIR_B, HIGH);
    analogWrite(PWM_B, SPD);
    delay(mDelay);
    oTime++;
  }
}
//-+-+-+>>>> Motor Driver section<<<<<<-+-+-+
Pages: 1 ... 4 5 [6] 7 8 9