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?
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
Post a Comment