program for Primo card and braccio sheild V4

Other Hardware Robotics
1

look a_head...

2 
base.write(r_base);base.read();
shoulder.write(a_epaule);shoulder.read();
elbow.write(a_coude);elbow.read();
wrist_rot.write(a_poignet);wrist_rot.read();
wrist_ver.write(r_poignet);wrist_ver.read();
gripper.write(m_pince);gripper.read();

Itmakesnosensetocramthecodetogetherwithoutspaces.

It makes no sense to read the servo positions when you don't store the result.

int vhti=40000;

Can 40000 actually fit in an int on a Primo card? Whatever that is...

3

I really progress know about controls thats new
hti and vhti is a time doing busy like millis() called maybe not more slow...
this program avoid a wall i just put one element but i want do more

#include <Braccio.h>
#include <Servo.h>
//EN COURS DE MODIF
//dimenssion
int Dsol=350;//mm
int SR_OB=95;//mm
int Dbase=170;//mm
int OB_OS=70;//mm
int OS_OE=125;//mm
int OE_OW=125;//mm
int OW_RP=60;//mm
int OW_PA=100;//mm
int PA_PF=120;//mm
int D_Art=25;//mm
Servo base;
int r_base=0;
int ba_base=0;
int bz_base=180;
int inc_b=2;
Servo shoulder;
int a_epaule=90;
int ba_epaule=45;
int bz_epaule=155;
int inc_e=10;
Servo elbow;
int a_coude=90;
int ba_coude=45;
int bz_coude=155;
int inc_c=-3;
Servo wrist_rot;
int a_poignet=92;
int ba_poignet=90;
int bz_poignet=180;
int inc_pa=4;
Servo wrist_ver;
int r_poignet=90;
int ba_rpoignet=0;
int bz_rpoignet=180;
int inc_pr=10;
Servo gripper;
int m_pince=10;//10 ouvert - 73 fermer
int ba_pince=10;
int bz_pince=73;
int inc_pm=10;

//if define movdx100 linex lecteur pourraitre etre pratique
int dx100[]={100,100,100,100,100,99,99,99,99,98,98,98,97,97,97,96,96,95,95,94,93,93,92,91,91,90,89,88,87,87,86,85,84,83,82,81,80,79,78,77,75,74,73,72,71};
int xbo=0;
int dy100[]={2,3,5,7,9,10,12,14,16,17,19,21,22,24,26,28,29,31,33,34,36,37,39,41,42,44,45,47,48,50,52,53,54,56,57,59,60,62,63,64,66,67,68,69,70};
int yco_n=0;
int dxa=0;

int d_tra=OW_PA+PA_PF;//220
int d_trb=PA_PF;
int d_wall=15000;//immaginaire min ça depend du pied du mur
int coef_plan=0;//rapport a la normale de travail

int in=0;
int jn=0;
int kn=0;
int sloon=2;
int hti=0;
int vhti=200;

void setup(){
Braccio.begin();
Serial.begin(9600);
base.write(r_base);base.read();
shoulder.write(a_epaule);shoulder.read();
elbow.write(a_coude);elbow.read();
wrist_rot.write(a_poignet);wrist_rot.read();
wrist_ver.write(r_poignet);wrist_ver.read();
gripper.write(m_pince);gripper.read();}

void loop(){
hti=hti+sloon;
//reaffecte les positions moteurs...
base.write(r_base);
base.read();
shoulder.read();
elbow.read();
wrist_rot.write(a_poignet);
wrist_rot.read();
wrist_ver.read();
gripper.read();

//base
in=r_base+1;//ejecteur
if(inc_b>0){in=bz_base-10;}
if(r_base>in){inc_b=2;}
if(r_base>bz_base){xbo=xbo+1;inc_b=-2;r_base=bz_base-1;}
in=r_base-1;
if(inc_b<0){in=ba_base+10;}
if(r_base<in){inc_b=-2;}
if(r_base<ba_base){inc_b=2;r_base=ba_base+1;}
if(hti>vhti){r_base=r_base+inc_b;}

//poignet
in=a_poignet+1;
if(inc_pa>0){in=bz_poignet-10;}//angle monte
if(a_poignet>in){sloon=1;inc_pa=1;}//slow
if(a_poignet>bz_poignet){a_poignet=bz_poignet-1;}
in=a_poignet-1;
if(inc_pa<0){in=ba_poignet+10;}//angle descend
if(a_poignet<in){sloon=1;inc_pa=-1;}//slow
if(a_poignet<ba_poignet){inc_pa=1;a_poignet=ba_poignet+1;}
if(hti>vhti){a_poignet=a_poignet+inc_pa;}//

//coef de rotation plaçons la normale a 90 comme si
if(hti>vhti){
in=r_base;
if(in>179){in=179;}if(in<0){in=0;}//limite
if(in<45){//0a44 az min-max h
coef_plan=dy100[in];in=0;}
if(in>134){//135 a 179 za max_min h
jn=179-in;coef_plan=dy100[jn];in=0;}
if(in>89){//90-134 az max-min l
jn=r_base-90;coef_plan=dx100[jn];in=0;}
if(in>44){//45-89 za min-max l
jn=89-r_base;coef_plan=dx100[jn];in=0;}}

//calcul des dxa sur un quart de cercle 90 180
if(hti>vhti){
in=a_poignet;
if(in<90){in=90;}if(in>179){in=179;}
if(in>134){//135_179 za
in=179-in;dxa=dx100[in];in=0;}
if(in>90){//90 134 sens az
in=in-90;dxa=dy100[in];in=0;}
dxa=dxa*d_tra;dxa=dxa/100;
in=0;jn=0;}

if(hti>vhti){
jn=dxa*coef_plan;//jn en centieme de milimètre
jn=jn+1;//15mm
if(jn>d_wall){//pre_collision
bz_poignet=a_poignet-1;
inc_pa=-4;
r_base=r_base-inc_b;}
if(jn<d_wall){//recherche max louche
bz_poignet=a_poignet+1;inc_pa=1;}
if(jn>1000){sloon=2;}
in=0;jn=0;
if(bz_poignet>180){bz_poignet=180;}}

if(hti>vhti){
in=d_wall/d_tra;
jn=dxa*coef_plan;jn=jn/in;
in=d_wall/in;
Serial.print(a_poignet);
Serial.print(",");
Serial.print(dxa);
Serial.print(",");
Serial.print(r_base);
Serial.print(",");
Serial.print(coef_plan);
Serial.print(",");
Serial.print(jn);
Serial.print(",");
Serial.println(in);
hti=0;}
if(xbo>1){delay(9000);}
}//fin du loop