How to delete IR controls, Add in IMU comment out LCD


i'm learning sure.  in thread suggestions getting rid of errors helping me gain insight on programming world.
in stewart platform code below (dont know works begin btw), i'd operate stewart platform via imu.  get in self leveling or stabilizing platform operation.

i dont need ir controls , dont need lcd.      i'd comment out lcd code - can go on learn how incorporate project later.


to start, can go in , delete lines (or perhaps comment them out ) associated ir controls?  

code: [select]

/* <controlling code arduino controlled rotary stewart platform>
    copyright (c) <2014>  <tomas korgo>

    this program free software: can redistribute and/or modify
    it under terms of gnu general public license published by
    the free software foundation, either version 3 of license, or
    (at option) later version.

    this program distributed in hope useful,
    but without warranty; without implied warranty of
    merchantability or fitness particular purpose.  see the
    gnu general public license more details.

    you should have received copy of gnu general public license
    along program.  if not, see <http://www.gnu.org/licenses/>.*/

#include <servo.h>
#include <wire.h>
#include <liquidcrystal_i2c.h>
#include <irremote.h>
#include <lcd.h>

//define values of irda codes send used remote control
#define mute_button 0x807f906f
#define standby_on 0x807f20df
#define vol_up 0x807f609f
#define vol_down 0x807fa05f
#define dvd 0x807f08f7
#define tv 0x807f8877
#define game 0x807f48b7
#define cd 0x807fc837

//define of characters used control of serial communication ['0'-'8']
#define setbackoff 48
#define setbackon 49
#define setpositions 50
#define printpos 51
#define stopprintpos 52
#define switchirda 53
#define setpositionsinms 54
#define switchirdaoff 55
#define geposition 56
//defines of lcd pin numbers, dont have changed except of i2c_addr value neccessary , have changed.
#define i2c_addr 0x27
#define lcd 1
#define irda 1
#define backlight_pin 3
#define en 2
#define rw 1
#define rs 0
#define d4 4
#define d5 5
#define d6 6
#define d7 7
//min , max pwm pulse sizes, can found in servo documentation
#define max 2200
#define min 800

//positions of servos mounted in opposite direction
#define inv1 1
#define inv2 3
#define inv3 5

//constants computation of positions of connection points
#define pi  3.14159
#define deg2rad 180/pi
#define deg30 pi/6
//variables used proper show of positions on lcd
char shown=0, showpos=0, useirda=0;
unsigned long time;

//variable store connected lcd
liquidcrystal_i2c lcd(i2c_addr, en, rw, rs, d4,d5,d6,d7);
//irreciever variables
irrecv irrecv(12); // receive ir on digital pin n 12
decode_results results;

//array of servo objects
servo servo[6];
//zero positions of servos, in positions arms horizontal, in us
static int zero[6]={1475,1470,1490,1480,1460,1490};
//in array stored requested position platform - x,y,z,rot(x),rot(y),rot(z)
static float arr[6]={0,0.0,0, radians(0),radians(0),radians(0)};
//actual degree of rotation of servo arms, start @ 0 - horizontal, used reduce
//complexity of calculating new degree of rotation
static float theta_a[6]={0.0,0.0,0.0, 0.0,0.0,0.0};
//array of current servo positions in us
static int servo_pos[6];
//rotation of servo arms in respect axis x
const float beta[] = {pi/2,-pi/2,-pi/6, 5*pi/6,-5*pi/6,pi/6},
//maximum servo positions, 0 horizontal position
      servo_min=radians(-80),servo_max=radians(80),
//servo_mult - multiplier used conversion radians->servo pulse in us
//l1-effective length of servo arm, l2 - length of base , platform connecting arm
//z_home - height of platform above base, 0 height of servo arms
servo_mult=400/(pi/4),l1 = 0.79,l2 = 4.66, z_home = 4.05;
//rd distance center of platform attachment points (arm attachment point)
//rd distance center of base center of servo rotation points (servo axis)
//theta_p-angle between 2 servo axis points, theta_r - between platform attachment points
//theta_angle-helper variable
//p[][]=x y values servo rotation points
//re[]{}=x y z values of platform attachment points positions
//equations used p , re affect postion of x axis, can changed achieve
//specific x axis position
const float rd = 2.42,pd =2.99,theta_p = radians(37.5),
theta_angle=(pi/3-theta_p)/2, theta_r = radians(8),
      p[2][6]={
          {
            -pd*cos(deg30-theta_angle),-pd*cos(deg30-theta_angle),
            pd*sin(theta_angle),pd*cos(deg30+theta_angle),
            pd*cos(deg30+theta_angle),pd*sin(theta_angle)
         },
         {
            -pd*sin(deg30-theta_angle),pd*sin(deg30-theta_angle),
            pd*cos(theta_angle),pd*sin(deg30+theta_angle),
            -pd*sin(deg30+theta_angle),-pd*cos(theta_angle)
         }
      },
      re[3][6] = {
          {
              -rd*sin(deg30+theta_r/2),-rd*sin(deg30+theta_r/2),
              -rd*sin(deg30-theta_r/2),rd*cos(theta_r/2),
              rd*cos(theta_r/2),-rd*sin(deg30-theta_r/2),
          },{
              -rd*cos(deg30+theta_r/2),rd*cos(deg30+theta_r/2),
              rd*cos(deg30-theta_r/2),rd*sin(theta_r/2),
              -rd*sin(theta_r/2),-rd*cos(deg30-theta_r/2),
          },{
              0,0,0,0,0,0
          }
};
//arrays used servo rotation calculation
//h[]-center position of platform can moved respect base, is
//translation vector representing move
static float m[3][3], rxp[3][6], t[3], h[3] = {0,0,z_home};

void setup(){
//lcd inicialisation , turning on backlight
#if lcd
   lcd.begin(16,2);
   lcd.setbacklightpin(backlight_pin, positive);
   lcd.setbacklight(high);
   lcd.home();
#endif

//attachment of servos pwm digital pins of arduino
   servo[0].attach(3, min, max);
   servo[1].attach(5, min, max);
   servo[2].attach(6, min, max);
   servo[3].attach(9, min, max);
   servo[4].attach(10, min, max);
   servo[5].attach(11, min, max);
//begin of serial communication
   serial.begin(9600);
//putting base position
   setpos(arr);
}

//function calculating needed servo rotation value
float getalpha(int *i){
   static int n;
   static float th=0;
   static float q[3], dl[3], dl2;
   double min=servo_min;
   double max=servo_max;
   n=0;
   th=theta_a[*i];
   while(n<20){
    //calculation of position of base attachment point (point on servo arm leg connected)
      q[0] = l1*cos(th)*cos(beta[*i]) + p[0][*i];
      q[1] = l1*cos(th)*sin(beta[*i]) + p[1][*i];
      q[2] = l1*sin(th);
    //calculation of distance between according platform attachment point , base attachment point
      dl[0] = rxp[0][*i] - q[0];
      dl[1] = rxp[1][*i] - q[1];
      dl[2] = rxp[2][*i] - q[2];
      dl2 = sqrt(dl[0]*dl[0] + dl[1]*dl[1] + dl[2]*dl[2]);
    //if distance same leg length, value of theta_a corrent, return it
      if(abs(l2-dl2)<0.01){
         return th;
      }
    //if not, split searched space in half, try next value
      if(dl2<l2){
         max=th;
      }else{
         min=th;
      }
      n+=1;
      if(max==servo_min || min==servo_max){
         return th;
      }
      th = min+(max-min)/2;
   }
   return th;
}






balance of code post above since exceeded 9000 characters.


code: [select]

//function calculating rotation matrix
void getmatrix(float pe[])
{
   float psi=pe[5];
   float theta=pe[4];
   float phi=pe[3];
   m[0][0] = cos(psi)*cos(theta);
   m[1][0] = -sin(psi)*cos(phi)+cos(psi)*sin(theta)*sin(phi);
   m[2][0] = sin(psi)*sin(phi)+cos(psi)*cos(phi)*sin(theta);

   m[0][1] = sin(psi)*cos(theta);
   m[1][1] = cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi);
   m[2][1] = cos(theta)*sin(phi);

   m[0][2] = -sin(theta);
   m[1][2] = -cos(psi)*sin(phi)+sin(psi)*sin(theta)*cos(phi);
   m[2][2] = cos(theta)*cos(phi);
}
//calculates wanted position of platform attachment poins using calculated rotation matrix
//and translation vector
void getrxp(float pe[])
{
   for(int i=0;i<6;i++){
      rxp[0][i] = t[0]+m[0][0]*(re[0][i])+m[0][1]*(re[1][i])+m[0][2]*(re[2][i]);
      rxp[1][i] = t[1]+m[1][0]*(re[0][i])+m[1][1]*(re[1][i])+m[1][2]*(re[2][i]);
      rxp[2][i] = t[2]+m[2][0]*(re[0][i])+m[2][1]*(re[1][i])+m[2][2]*(re[2][i]);
   }
}
//function calculating translation vector - desired move vector + home translation vector
void gett(float pe[])
{
   t[0] = pe[0]+h[0];
   t[1] = pe[1]+h[1];
   t[2] = pe[2]+h[2];
}

unsigned char setpos(float pe[]){
    unsigned char errorcount;
    errorcount=0;
    for(int = 0; < 6; i++)
    {
        gett(pe);
        getmatrix(pe);
        getrxp(pe);
        theta_a[i]=getalpha(&i);
        if(i==inv1||i==inv2||i==inv3){
            servo_pos[i] = constrain(zero[i] - (theta_a[i])*servo_mult, min,max);
        }
        else{
            servo_pos[i] = constrain(zero[i] + (theta_a[i])*servo_mult, min,max);
        }
    }

    for(int = 0; < 6; i++)
    {
        if(theta_a[i]==servo_min||theta_a[i]==servo_max||servo_pos[i]==min||servo_pos[i]==max){
            errorcount++;
        }
        servo[i].writemicroseconds(servo_pos[i]);
    }
    return errorcount;
}

//functions used displaying actual platform position on 16x2 lcd display
#if lcd
void showrot(){
   lcd.setcursor(0,0);
   lcd.print("rot");
   lcd.setcursor(12,0);
   lcd.print((int)(arr[3]*deg2rad));
   lcd.setcursor(3,1);
   lcd.print((int)(arr[4]*deg2rad));
   lcd.setcursor(11,1);
   lcd.print((int)(arr[5]*deg2rad));
}
void showcomm(){
   if(shown==0){
      shown=1;
      lcd.setcursor(3,0);
      lcd.print("ation x: ");
      lcd.setcursor(0,1);
      lcd.print("y: ");
      lcd.setcursor(8,1);
      lcd.print("z: ");
   }
}
void clearnr(){
   lcd.setcursor(12,0);
   lcd.print("    ");
   lcd.setcursor(3,1);
   lcd.print("     ");
   lcd.setcursor(11,1);
   lcd.print("     ");

}
void showloc(){
   lcd.setcursor(0,0);
   lcd.print("loc");
   lcd.setcursor(12,0);
   lcd.print((int)(arr[0]*25.4));
   lcd.setcursor(3,1);
   lcd.print((int)(arr[1]*25.4));
   lcd.setcursor(11,1);
   lcd.print((int)(arr[2]*25.4));
}
#endif

//main control loop, obtain requested action serial connection, execute it
void loop()
{

   if(serial.available()>0){
      int input=serial.read();
      switch(input){
//action turn backlight off
         case setbackoff:
#if lcd
            lcd.setbacklight(low);
#endif
            break;
//action turn backlight on
         case setbackon:
#if lcd
            lcd.setbacklight(high);
#endif
            break;
//action change position of platform, obtain 6 values representing desired position
         case setpositions:
            for(int i=0;i<6;i++){
               long kk;
               while(serial.available()<4){
                  ;
               }
               kk=(long)serial.read();
               kk=kk+(serial.read()<<8);
               kk=kk+(serial.read()<<16);
               kk=kk+(serial.read()<<24);
               if(i<3){
                  arr[i]=(kk/100)/25.4;
               }else{
                  arr[i]=radians(kk/100.0);
               }
            }
            serial.write(setpos(arr));
            serial.flush();
            break;
//enable of showing current position on lcd
         case printpos:
#if lcd
            showpos=printpos;
            time=millis();
#endif
            break;
//enable of controlling platformy irda remote
        case switchirda:
#if irda
            irrecv.enableirin();
            useirda=switchirda;
#endif
            break;
//reserved future use - possiblity send servo timing values
//main control executed on communicating partner
         case setpositionsinms:
            for(int i=0;i<6;i++){
               long kk;
               while(serial.available()<4){
                  ;
               }
               kk=(long)serial.read();
               kk=kk|(serial.read()<<8);
               kk=kk|(serial.read()<<16);
               kk=kk|(serial.read()<<24);
               servo[i].writemicroseconds(kk);
            }
            break;
//disable of showing current position on lcd
         case stopprintpos:
            showpos=stopprintpos;
            shown=0;
            break;
//disable of controlling platformy irda remote
         case switchirdaoff:
            useirda=switchirdaoff;
            break;
//return current position of platform
         case geposition:
            retpos();
            break;
         default:
            break;
      }
   }
//helping subroutine print current position
#if lcd
   if(showpos==printpos){
      static byte act=0;
      showcomm();
      if(millis()-time<1500){
         act=0;
      }else if(millis()-time<3000){
         if(act==0){
            act=1;
            clearnr();
            showrot();
         }
      }else{
         time=millis();
         clearnr();
         showloc();
      }
   }
#endif
//this part used irda position control
#if irda
   if(useirda==switchirda&&irrecv.decode(&results)){
      static byte val=0;
      switch(results.value){
         case mute_button:
            val=1;
            break;
         case standby_on:
            val=0;
            break;
         case vol_up:
            if(val<=2){
               arr[val]+=1;
            }else{
               arr[val]=radians((arr[val]*deg2rad)+0.4);
            }
            break;
         case vol_down:
            if(val<=2){
               arr[val]-=1;
            }else{
               arr[val]=radians((arr[val]*deg2rad)-0.4);
            }
            break;
         case dvd:
            val=2;
            break;
         case tv:
            val=3;
            break;
         case game:
            val=4;
            break;
         case cd:
            val=5;
            break;
      }
      setpos(arr);
      irrecv.resume(); // continue receiving
   }
#endif

}

void retpos(){
   for(int i=0;i<6;i++){
       long val;
       if(i<3){
           val=(long)(arr[i]*100*25.4);
       }else{
           val=(long)(arr[i]*100*deg2rad);
       }
       serial.write(val);
       serial.write((val>>8));
       serial.write((val>>16));
       serial.write((val>>24));
       serial.flush();
   }
}





Arduino Forum > Using Arduino > Installation & Troubleshooting > How to delete IR controls, Add in IMU comment out LCD


arduino

Comments

Popular posts from this blog

Flip address is out of range arduino uno r3

Arduino Uno not uploading

Indesign and MathType fonts