Components and supplies
Arduino Mega 2560
Adafruit 16ch servo driver
GY-521 MPU-6050 3 Axis Gyroscope + Accelerometer Module For Arduino
LIDAR Laser
xTion PRO live
Servos (Tower Pro MG996R)
Raspberry Pi 3 Model B
Tools and machines
Amateur tools and machines and offcourse, a lot of work.
Project description
Code
basic example code
arduino
1//tratta da 3056C 2 3//e spinta zampe posteriori forse i polpacci si devono aprire prima!! 4 5//LIBRERIA WIRE MODIFICATA! riferirsi al sito per twi.c e twi.h modificate e istruzioni! 6//ALTRA LIBREIRA MODIFICATA : MPU6050_6Axis_MotionApps20.h 7//VALORI UTILIZZATI : 0x02, 0x16, 0x02, 0x00, 0x03 // D_0_22 inv_set_fifo_rate 8 9 10/* 11prova a : 12 13A) Se il robot sta perdendo l'equilibrio durante fase ricarica: BLOCCARE RICARICA E PENSARE AD EQUILIBRIO!! 14Magari come inizia fase ricarica, il robot puo' alzare leggermente la zampa per vedere se perde l'equilibrio! 15Se SI interrompe e la zampa,dato che era appena alzata, torna subito al suo posto! 16Se NO allora continua la fase ricarica! 17 18B) la zampa che ha appena caricato NON deve essere utilizzata per finire l'equilibrio fatto in pre ricarica 19 20C) Provare a mettere il peso del robot nella zampa che ha appena ricaricato in fase di spinta! 21 22 23 24 25 E) RIDURRE VALORE NUMERICO ANGOLI ROLL E PITCH! 26 ora sono troppo alti per l'inclinazione reale! 27 RISULTATO: 28 SE SI RIDUCE IN 90/M_PI, I GRADI AUMENTANO E DIMINUISCONO MENO DURANTE INCLINAZIONE! 29 Primo pensiero che viene e' che, con le attuali routine, si perde di precisione nell' 30 equilibrio del robot! ma ci sarebbe da approfondire il discorso e magari provare 31 un valore tra 180 e 90, diciamo 135 per trovare un compromesso che ci soddisfi piu' dell' 32 equilibrio settato con 180/M_PI 33 34 35D) e considera anche che potresti fare che una zampa quando 36troppo allungata (tipo zampe posteriori in spinta) allora 37riduce o proprio non fa movimento equilibrio! 38 39 40 41*/ 42 43/* 44fare una routine che equilibria il robot, SOLO(?) in fase di ricarica, utilizzando i sensori tattili 45e cercando di portare, se ad esempio si sta caricando zampa 1, tale zampa a un leggerissimo 46contatto con il terreno rispetto alle altre zampe. 47Sarebbe gia' pronta per caricare senza rischio di far cadere il robot 48posso gia' usare i tasti set target su processing! 49 50EQUILIBRIO ZAMPA 1: 51A) prima si agisce lateralmente, facendo inclinare JQR a sinistra aspettando che la pressione 52della zampa 1 entri nel "leggerissimo" 53 54B) Se l'inclinazione laterale non basta, parte l'inclinazione indietro. A fine corsa si dovrebbe 55 aver ottenuto la zampa 1 con pressione nel terreno "leggerissima" 56 57 58A e B possono anche partire e avanzare insieme eh! 59 60oppure: 61ALZARE LEGGERMENTE (braccio+avambraccio) LA ZAMPA OPPOSTA (quindi zampa 4) COME 62QUANDO SI RICARICA MA SOLTANTO DI POCHISSIMO, IL TANTO CHE BASTA PER FARE 63UNA PICCOLA CADUTA DALLA PARTE OPPOSTA ALLA ZAMPA CHE DEVE RICARICARE (LA 1) 64*/ 65 66 67#include "MemoryFree.h" 68//#include <SoftwareSerial.h> 69//#include "avr/wdt.h" WATCHDOG 70 71 72//--------------------------------- GYRO 73#include "I2Cdev.h" 74#include "MPU6050_6Axis_MotionApps20.h" 75#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 76 #include "Wire.h" 77 //#include "NBWire.h" 78#endif 79 80MPU6050 accelgyro(0x69); //serve? cmq prova anche con 69 81 82MPU6050 mpu; 83#define OUTPUT_READABLE_YAWPITCHROLL 84//#define OUTPUT_READABLE_QUATERNION 85 86//#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) 87bool blinkState = false; 88// MPU control/status vars 89bool dmpReady = false; // set true if DMP init was successful 90uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 91uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 92uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 93uint16_t fifoCount; // count of all bytes currently in FIFO 94uint8_t fifoBuffer[64]; // FIFO storage buffer 95// orientation/motion vars 96Quaternion q; // [w, x, y, z] quaternion container 97VectorInt16 aa; // [x, y, z] accel sensor measurements 98VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements 99VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements 100VectorFloat gravity; // [x, y, z] gravity vector 101float euler[3]; // [psi, theta, phi] Euler angle container 102float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 103// packet structure for InvenSense teapot demo 104//uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; 105// ================================================================ 106// === INTERRUPT DETECTION ROUTINE === 107// ================================================================ 108volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 109void dmpDataReady() { 110 mpuInterrupt = true; 111} 112 113 114 115#include <Adafruit_PWMServoDriver.h> 116 117// called this way, it uses the default address 0x40 118Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); 119// you can also call it with a different address you want 120//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41); 121 122// Depending on your servo make, the pulse width min and max may vary, you 123// want these to be as small/large as possible without hitting the hard stop 124// for max range. You'll have to tweak them as necessary to match the servos you 125// have! 126 127// ZAMPA 1 -AVANTI DESTRA 128//AVAMBRACCIO 1 129#define SERVOMIN_ava1 200 //180 - 630 130#define SERVOMAX_ava1 650 131 132//BRACCIO 1 -- ORA E' SPALLA ZAMPA 4 133#define SERVOMIN_bra1 165 //180 134#define SERVOMAX_bra1 615 // 640 135 136//SPALLA 1 137#define SERVOMIN_spa1 180 //330-600 piu aumenta piu la spalla si allarga 138#define SERVOMAX_spa1 630 139 140//------------------------------------------------------------------- 141 142// ZAMPA 2 -AVANTI SINISTRA 143//AVAMBRACCIO 2 144#define SERVOMIN_ava2 150 //era 190 640 ora devo usare 160 610 alzandolo si abbassa l'avambraccio 145#define SERVOMAX_ava2 600 146 147//BRACCIO 2 148#define SERVOMIN_bra2 165 //braccio deve andare leggerm avanti riducendo il valore! 149#define SERVOMAX_bra2 615 150 151//SPALLA 2 152#define SERVOMIN_spa2 100 //120 153#define SERVOMAX_spa2 550 //570 154 155//------------------------------------------------------------------- 156 157// ZAMPA 3 -DIETRO DESTRA 158//AVAMBRACCIO 3 159#define SERVOMIN_ava3 150 160#define SERVOMAX_ava3 600 161 162//BRACCIO 3 163#define SERVOMIN_bra3 150 //140-590 164#define SERVOMAX_bra3 600 165 166//SPALLA 3 167#define SERVOMIN_spa3 70 //90-540 168#define SERVOMAX_spa3 520 169 170//------------------------------------------------------------------- 171 172// ZAMPA 4 -DIETRO SINISTRA 173//AVAMBRACCIO 4 174#define SERVOMIN_ava4 150 175#define SERVOMAX_ava4 600 //prova abbassandolo 176 177//BRACCIO 4 178#define SERVOMIN_bra4 130 //164-600 alzando il valore torna indietro 179#define SERVOMAX_bra4 580 180 181//SPALLA 4 182#define SERVOMIN_spa4 90 //90-540 183#define SERVOMAX_spa4 540 184 185 186// our servo # counter 187uint8_t servonum = 0; 188 189 190//---------------------------- SENSORI TATTILI ----------------------------- 191 int fsrAnalogPin1 = 0; // FSR is connected to analog 0 192 int fsrAnalogPin2 = 1; // FSR is connected to analog 1 193 int fsrAnalogPin3 = 2; // FSR is connected to analog 2 194 int fsrAnalogPin4 = 3; // FSR is connected to analog 3 195 int fsrReading1; // the analog reading from the FSR resistor divider 196 int fsrReading2; // the analog reading from the FSR resistor divider 197 int fsrReading3; // the analog reading from the FSR resistor divider 198 int fsrReading4; // the analog reading from the FSR resistor divider 199 int fsrReading1_check = 0, fsrReading2_check = 0, fsrReading3_check = 0, fsrReading4_check = 0; 200//-------------------------------------------------------------------------- 201//per camminata furtiva 202int fase_mov_zampe = 0, fase_mov_zampa2 = 0, fase_mov_zampa3 = 0, fase_mov_zampa4 = 0; 203String equilibrio_str = ""; 204 205//per camminata test (furtiva 2) 206int routine_equilibrio = 1;// 1 --> on / 0 --> off routine equilibrio 207int target_x = 0, target_y = 0; 208int valore_tar_sin2 = 0,valore_tar_sin4 = 0, valore_tar_des1 = 0, valore_tar_des3 = 0; 209int valore_tar_sin2b = 0,valore_tar_sin4b = 0, valore_tar_des1b = 0, valore_tar_des3b = 0; 210int valore_tar_des1_SOMMA = 0, valore_tar_des3_SOMMA = 0, valore_tar_sin2_SOMMA = 0, valore_tar_sin4_SOMMA = 0; 211int valore_tar_des1_FIN = 0, valore_tar_des3_FIN = 0, valore_tar_sin2_FIN = 0, valore_tar_sin4_FIN = 0; 212int valore_vel_des1 = 0, valore_vel_des3 = 0, valore_vel_sin2 = 0, valore_vel_sin4 = 0; 213int valore_vel_des1b = 0, valore_vel_des3b = 0, valore_vel_sin2b = 0, valore_vel_sin4b = 0; 214int valore_vel_des1_FIN = 0, valore_vel_des3_FIN = 0, valore_vel_sin2_FIN = 0, valore_vel_sin4_FIN = 0; 215int dist_angolo_roll_con_target_x; 216int dist_angolo_pitch_con_target_y; 217int velocita_mov, velocita_movb; 218int flag_kfin = 0; 219int pillozzo = 0; 220int pillozzoB = 0; 221int timer_raggiung_target_x = 0; 222int timer_raggiung_target_y = 0; 223int timer_generico = 0, timer_generico2 = 0, timer_generico3 = 0; 224//int timer_generico3 = 0; 225int durata_timer = 0; 226int target_x_appo = 0; 227int target_y_appo = 0; 228int tempo_attesa_appo = 0; 229int durata_spinte = 0; 230int timer_check_assi = 0; 231int fase_equilibrio = 0; 232int inclinazione_a = 0; //da togliere 233int zona_velocita = 0; 234int fase_equilibrio_B = 0, fase_equilibrio_C = 0; 235int zampa1_no_equilibrio = 0, zampa2_no_equilibrio = 0, zampa3_no_equilibrio = 0, zampa4_no_equilibrio = 0; 236int speed_zona_velocita1 , speed_zona_velocita2; 237int riflessi_massimi = 1; 238int spinta_zampe = 0; 239 240int targ_crono_vel_equilib = 0; 241 242int pulselengthz; 243int pulselengthzb; 244int pulselengthzc; 245 246int new_pos_braccio1; 247int new_pos_braccio2; 248int new_pos_avambraccio1; 249int new_pos_avambraccio2; 250 251 252int new_pos_spalla1; 253int new_pos_spalla2; 254int new_pos_spalla3; 255int new_pos_spalla4; 256int new_pos_braccio4; 257int new_pos_avambraccio3; 258int new_pos_avambraccio4; 259 260 261//-------------------- CONFIGURAZIONE INIZIALE ZAMPE >< 262//-------------------------------ZAMPA NUMERO 1 - AVANTI DESTRA--------------------------- 263//AVAMBRACCIO 1 264int pos_avambraccio1 = 60; // 140=MAX (avambr tutto aperto)// 60 265 // 0=MIN (avambr tutto chiuso) 266 // 70=MED 267//BRACCIO 1 268int pos_braccio1 = 115; // 140=MAX(braccio tutto dietro // 115 269 // 0=MIN (braccio tutto avanti) 270 // 70=MED 271//SPALLA 1 272int pos_spalla1 = 60; //120=MAX (spalla tutta chiusa) 95 -valore minore chiude ascella 273 // 0=MIN (spalla tutta aperta) 274 //60 = MED 275 276//-------------------------------ZAMPA NUMERO 2 - AVANTI SINISTRA--------------------------- 277//AVAMBRACCIO 2 278int pos_avambraccio2 = 60; // 140=MAX (avambr tutto aperto) 60 279 // 0=MIN (avambr tutto chiuso) 280 // 70=MED 281//BRACCIO 2 282int pos_braccio2 = 115; // 140=MAX(braccio tuto dietro 115 283 // 0=MIN (braccio tutto avanti) 284 // 70=MED 285//SPALLA 2 286int pos_spalla2 = 60; // 120 =MAX (spalla tutta chiusa) 287 // 0 =MIN (spalla tutta aperta) 288 // 60 = MED 289 290 291 292 293//-------------------------------ZAMPA NUMERO 3 - DIETRO DESTRA--------------------------- 294//AVAMBRACCIO 3 295int pos_avambraccio3 = 60; // 140=MAX (avambr tutto aperto) 60 296 // 0=MIN (avambr tutto chiuso) 297 // 70=MED OK 60 298 299//BRACCIO 3 300int pos_braccio3 = 35; // 140=MAX(braccio tutto dietro 35 301 // 0=MIN (braccio tutto avanti) 302 // 70=MED 303//SPALLA 3 304int pos_spalla3 = 60; //120=MAX (spalla tutta chiusa) 305 // 0=MIN (spalla tutta aperta) 306 //60 = MED 307 308//-------------------------------ZAMPA NUMERO 4 - DIETRO SINISTRA--------------------------- 309//AVAMBRACCIO 4 310int pos_avambraccio4 = 60; // 140=MAX (avambr tutto aperto) 60 311 // 0=MIN (avambr tutto chiuso) 312 // 70=MED 313//BRACCIO 4 314int pos_braccio4 = 35; // 140=MAX(braccio tutto dietro 35 315 // 0=MIN (braccio tutto avanti) 316 // 70=MED 317//SPALLA 4 318int pos_spalla4 = 60; //120=MAX (spalla tutta chiusa) 319 // 0=MIN (spalla tutta aperta) 320 //60 = MED 321 322 323/* 324//-------------------- CONFIGURAZIONE INIZIALE A MO DI SPRINTER IN POSIZIONE 325//-------------------------------ZAMPA NUMERO 1 - AVANTI DESTRA--------------------------- 326//AVAMBRACCIO 1 327int pos_avambraccio1 = 60; // 140=MAX (avambr tutto aperto)// 80 328 // 0=MIN (avambr tutto chiuso) 329 // 70=MED 330//BRACCIO 1 331int pos_braccio1 = 130; // 140=MAX(braccio tutto dietro // 90 332 // 0=MIN (braccio tutto avanti) 333 // 70=MED 334//SPALLA 1 335int pos_spalla1 = 60; //160=MAX (spalla tutta chiusa) 85 336 // 0=MIN (spalla tutta aperta) 337 //90 = MED 338 339//-------------------------------ZAMPA NUMERO 2 - AVANTI SINISTRA--------------------------- 340//AVAMBRACCIO 2 341int pos_avambraccio2 = 65; // 140=MAX (avambr tutto aperto) 342 // 0=MIN (avambr tutto chiuso) 343 // 70=MED 344//BRACCIO 2 345int pos_braccio2 = 105; // 140=MAX(braccio tuto dietro 60 346 // 0=MIN (braccio tutto avanti) 347 // 70=MED 348//SPALLA 2 349int pos_spalla2 = 60; //160=MAX (spalla tutta chiusa) 350 // 0=MIN (spalla tutta aperta) 351 //90 = MED 352 353 354 355 356//-------------------------------ZAMPA NUMERO 3 - DIETRO DESTRA--------------------------- 357//AVAMBRACCIO 3 358int pos_avambraccio3 = 55; // 140=MAX (avambr tutto aperto) 6 359 // 0=MIN (avambr tutto chiuso) 360 // 70=MED 361 362//BRACCIO 3 363int pos_braccio3 = 25; // 140=MAX(braccio tutto dietro 60 364 // 0=MIN (braccio tutto avanti) 365 // 70=MED 366//SPALLA 3 367int pos_spalla3 = 60; //160=MAX (spalla tutta chiusa) 368 // 0=MIN (spalla tutta aperta) 369 //90 = MED 370 371//-------------------------------ZAMPA NUMERO 4 - DIETRO SINISTRA--------------------------- 372//AVAMBRACCIO 4 373int pos_avambraccio4 = 65; // 140=MAX (avambr tutto aperto) 60 374 // 0=MIN (avambr tutto chiuso) 375 // 70=MED 376//BRACCIO 4 377int pos_braccio4 = 45; // 140=MAX(braccio tutto dietro 60 45 378 // 0=MIN (braccio tutto avanti) 379 // 70=MED 380//SPALLA 4 381int pos_spalla4 = 60; //160=MAX (spalla tutta chiusa) 382 // 0=MIN (spalla tutta aperta) 383 //90 = MED 384*/ 385 386 387 388//int ginolillo = 0; 389int pos_avambraccio1_fin = pos_avambraccio1; 390int pos_braccio1_fin = pos_braccio1; 391int pos_spalla1_fin = pos_spalla1; 392int pos_avambraccio2_fin = pos_avambraccio2; 393int pos_braccio2_fin = pos_braccio2; 394int pos_spalla2_fin = pos_spalla2; 395int pos_avambraccio3_fin = pos_avambraccio3; 396int pos_braccio3_fin = pos_braccio3; 397int pos_spalla3_fin = pos_spalla3; 398int pos_avambraccio4_fin = pos_avambraccio4; 399int pos_braccio4_fin = pos_braccio4; 400int pos_spalla4_fin = pos_spalla4; 401 402int appo_pos_braccio1_fin, appo_pos_avambraccio1_fin, appo_pos_spalla1_fin; 403int appo_pos_braccio2_fin, appo_pos_avambraccio2_fin, appo_pos_spalla2_fin; 404int appo_pos_braccio3_fin, appo_pos_avambraccio3_fin, appo_pos_spalla3_fin; 405int appo_pos_braccio4_fin, appo_pos_avambraccio4_fin, appo_pos_spalla4_fin; 406int ok_leg_back_position = 0; 407 408 409int val_stand_attesa = 2; 410int timer_bra1 = 0, attesa_bra1 = val_stand_attesa; 411int timer_ava1 = 0, attesa_ava1 = val_stand_attesa; 412int timer_spa1 = 0, attesa_spa1 = val_stand_attesa; 413int timer_bra2 = 0, attesa_bra2 = val_stand_attesa; 414int timer_ava2 = 0, attesa_ava2 = val_stand_attesa; 415int timer_spa2 = 0, attesa_spa2 = val_stand_attesa; 416int timer_bra3 = 0, attesa_bra3 = val_stand_attesa; 417int timer_ava3 = 0, attesa_ava3 = val_stand_attesa; 418int timer_spa3 = 0, attesa_spa3 = val_stand_attesa; 419int timer_bra4 = 0, attesa_bra4 = val_stand_attesa; 420int timer_ava4 = 0, attesa_ava4 = val_stand_attesa; 421int timer_spa4 = 0, attesa_spa4 = val_stand_attesa; 422 423int moto_zampa1 = 0; // 0 = zampa eseguira' codice - 10 = zampa disabilitata 424int moto_zampa2 = 0; 425int moto_zampa3 = 0; 426int moto_zampa4 = 0; 427 428//--------------------------------------------------------------------------------------------------------------------- 429//--------------------------------------------------------------------------------------------------------------------- 430//--------------------------------------------------------------------------------------------------------------------- 431//fai ricarica zampa3! prendi dati posizione zampe nel momento in cui ricarica 432//la zampa 3 e poi crea la scenetta per provare nuova ricarica (che devi ancora fare!) 433 434 435int start = 7; //7; //0= AVVIENE SOLO IL SETTAGGIO INIZIALE - 5= MOVIMENTI ZAMPE TROTTO -7= MOVIMENTO FURTIVO - 10= TEST 436int velocita_spinta; //12 437//--------------------------------------------------------------------------------------------------------------------- 438//--------------------------------------------------------------------------------------------------------------------- 439//--------------------------------------------------------------------------------------------------------------------- 440 441int camminata_test = 0; 442int incomingByte; 443int reset_saltelli = 0; 444int timer_ricarica1 = 0, timer_ricarica2 = 0, timer_ricarica3 = 0, timer_ricarica4 = 0; 445int posiz_appo = 0; 446int posiz_appo2 = 0; 447 448// 0= spinta / 1= ricarica / 10 = disabilitata spinta 449int ok_zampa1 = 0; 450int ok_zampa2 = 0; 451int ok_zampa3 = 0; 452int ok_zampa4 = 0; 453int pausa = 0; 454int movimento_on = -1; 455int movimento_on_flag = 0;//serve nell'attesa prima dello show! 456int angolo_rotta_target = 0; 457int secondi; 458int fase = 0; 459String risposta_direzione; 460 461// angolo Z - YAWN BUSSOLA 462int angolo_rotta_attuale_reale = 0; 463int angolo_rotta_attuale = 0; 464int conto_rotta = 0; 465int angolo_rotta_attuale_appo1 = 0, angolo_rotta_attuale_appo2 = 0, angolo_rotta_attuale_appo3 = 0; 466 467// angolo X - LATERAL ROLL 468float angolo_roll_reale = 0; 469int angolo_roll_attuale = 0; 470int conto_roll = 0; 471int angolo_roll_appo1 = 0, angolo_roll_appo2 = 0, angolo_roll_appo3 = 0; 472 473// angolo Y - PITCH 474int angolo_pitch_reale = 0; 475int angolo_pitch_attuale = 0; 476int conto_pitch = 0; 477int angolo_pitch_appo1 = 0, angolo_pitch_appo2 = 0, angolo_pitch_appo3 = 0; 478 479int z; //asse z gyro -yaw- RAW (bussola) 480int x; //asse x gyro -roll- RAW 481int y; //asse y gyro -pitch- RAW 482 483 484//------------------------------------ ARRAY VISTA 485 486byte array_vista[461]; 487int i = 1; 488 489int timer_for = 0; 490 491 492int secondi_test; 493int decimi_test; 494int centesimi_test; 495int loading_scansione = 0; 496int index = 0; 497 498 499//____________________________________________SETTING INIZIALE_________________________________________ 500void setup() 501{ 502 503 //Memorizzazione INIZIALE attuale posizione zampe 504 pos_braccio2_fin = pos_braccio2; 505 pos_avambraccio2_fin = pos_avambraccio2; 506 507 for(i = 1; i <= 461; i = i + 1) 508 { 509 array_vista[i] = 0; 510 } 511 512 i = 1; //pronta per dopo! 513 514 515 /* 516 if(timer_for == 0) 517 { 518 for(i = 1; i < 121; i = i + 1) 519 { 520 //Serial.println(myPins[i]); 521 array_vista[i] = -1; 522 523 } 524 timer_for = 1; 525 } 526 */ 527 528 //wdt_enable(WDTO_2S); WATCHDOG 529 //-------------------------- GYRO 530 // join I2C bus (I2Cdev library doesn't do this automatically) 531 #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 532 Wire.begin(); 533 TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz) 534 #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 535 Fastwire::setup(200, true); //o 400???? 536 #endif 537 Serial.begin(115200); //115200 538 //Serial.begin(57600); 539 while (!Serial); // wait for Leonardo enumeration, others continue immediately 540 mpu.initialize(); 541 // load and configure the DMP 542 //Serial.println(F("Initializing DMP...")); 543 devStatus = mpu.dmpInitialize(); 544 // supply your own gyro offsets here, scaled for min sensitivity 545 mpu.setXGyroOffset(220); 546 mpu.setYGyroOffset(76); 547 mpu.setZGyroOffset(-85); 548 mpu.setZAccelOffset(1788); //1788 // 1688 factory default for my test chip 549 // make sure it worked (returns 0 if so) 550 if (devStatus == 0) { 551 mpu.setDMPEnabled(true); 552 // enable Arduino interrupt detection 553 //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); 554 attachInterrupt(0, dmpDataReady, RISING); 555 mpuIntStatus = mpu.getIntStatus(); 556 // set our DMP Ready flag so the main loop() function knows it's okay to use it 557 //Serial.println(F("DMP ready! Waiting for first interrupt...")); 558 dmpReady = true; 559 // get expected DMP packet size for later comparison 560 packetSize = mpu.dmpGetFIFOPacketSize(); 561 } else { 562 563 } 564 // configure LED for output 565 //pinMode(LED_PIN, OUTPUT); 566 567 568 pwm.begin(); 569 pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates 570 571 572//30 170 --- 10 150 (se invertito) 573//DEVI CONVERTIRE PER OTTENERE DI POTER ORDINARE MOVIMENTI DA 0 a 140 574 575 //************************************************************************************** ZAMPA 1 SETUP 576 //SETTAGGIO INIZIALE AVAMBRACCIO - 0 577 //pos_avambraccio1 = pos_avambraccio1 + 10; //+10 perche' e'in mirror se no sarebbe +30 578 new_pos_avambraccio1 = 180 - pos_avambraccio1; 579 pulselengthz = map(new_pos_avambraccio1, 0, 180, SERVOMIN_ava1, SERVOMAX_ava1); 580 pwm.setPWM(0, 0, pulselengthz); 581 582 //SETTAGGIO INIZIALE BRACCIO - 1 583 //pos_braccio1 = pos_braccio1 + 10; //+10 perche' e'in mirror se no sarebbe +30 584 //new_pos_braccio1 = 180 - pos_braccio1; 585 pulselengthzb = map(pos_braccio1, 0, 180, SERVOMIN_bra1, SERVOMAX_bra1); 586 pwm.setPWM(1, 0, pulselengthzb); 587 588 //SETTAGGIO INIZIALE SPALLA - 2 589 /* 590 new_pos_spalla1 = 180 - pos_spalla1; 591 pulselengthzc = map(new_pos_spalla1, 0, 180, SERVOMIN_spa1, SERVOMAX_spa1); 592 pwm.setPWM(2, 0, pulselengthzc); 593 */ 594 //SETTAGGIO INIZIALE SPALLA - 2 595 new_pos_spalla1 = 180 - pos_spalla1; 596 pulselengthzc = map(new_pos_spalla1, 0, 180, SERVOMIN_spa1, SERVOMAX_spa1); 597 pwm.setPWM(2, 0, pulselengthzc); 598 599 600 601 //************************************************************************************** ZAMPA 2 SETUP 602 //SETTAGGIO INIZIALE AVAMBRACCIO - 4 603 //pos_avambraccio2 = pos_avambraccio2 + 30; //se fosse in mirror sarebbe +10 604 new_pos_avambraccio2 = 180 - pos_avambraccio2; 605 pulselengthz = map(new_pos_avambraccio2, 0, 180, SERVOMIN_ava2, SERVOMAX_ava2); 606 pwm.setPWM(4, 0, pulselengthz); 607 608 //SETTAGGIO INIZIALE BRACCIO - 6 609 //pos_braccio2 = pos_braccio2 + 10; //se fosse in mirror sarebbe +10 610 new_pos_braccio2 = 180 - pos_braccio2; 611 pulselengthzb = map(new_pos_braccio2, 0, 180, SERVOMIN_bra2, SERVOMAX_bra2); 612 pwm.setPWM(6, 0, pulselengthzb); 613 614 615 616 //SETTAGGIO INIZIALE SPALLA - 7 617 new_pos_spalla2 = 180 - pos_spalla2; 618 pulselengthzc = map(new_pos_spalla2, 0, 180, SERVOMIN_spa2, SERVOMAX_spa2); 619 pwm.setPWM(7, 0, pulselengthzc); 620 621 622 623 624 625 626 627 //************************************************************************************** ZAMPA 3 SETUP 628 //SETTAGGIO INIZIALE AVAMBRACCIO - 8 629 pos_avambraccio3 = pos_avambraccio3 + 0;// e' mirror quindi + 10 630 new_pos_avambraccio3 = 180 - pos_avambraccio3; 631 pulselengthz = map(new_pos_avambraccio3, 0, 180, SERVOMIN_ava3, SERVOMAX_ava3); 632 pwm.setPWM(8, 0, pulselengthz); 633 634 //SETTAGGIO INIZIALE BRACCIO - 9 635 //pos_braccio3 = pos_braccio3 + 30;//non e' mirror quindi + 30 636 //new_pos_braccio3 = 190 - pos_braccio3; 637 pulselengthzb = map(pos_braccio3, 0, 180, SERVOMIN_bra3, SERVOMAX_bra3); 638 pwm.setPWM(9, 0, pulselengthzb); 639 640 641 //SETTAGGIO INIZIALE SPALLA - 10 642 new_pos_spalla3 = 180 - pos_spalla3; 643 pulselengthzc = map(new_pos_spalla3, 0, 180, SERVOMIN_spa3, SERVOMAX_spa3); 644 pwm.setPWM(10, 0, pulselengthzc); 645 646 647 648 //************************************************************************************** ZAMPA 4 SETUP 649 //SETTAGGIO INIZIALE AVAMBRACCIO - 12 650 //pos_avambraccio4 = pos_avambraccio4 + 10;//e' mirror quindi + 10 651 //new_pos_avambraccio4 = 180 - pos_avambraccio4; 652 new_pos_avambraccio4 = pos_avambraccio4; 653 //new_pos_avambraccio4 = pos_avambraccio4; 654 pulselengthz = map(new_pos_avambraccio4, 0, 180, SERVOMIN_ava4, SERVOMAX_ava4); 655 pwm.setPWM(12, 0, pulselengthz); 656 657 //SETTAGGIO INIZIALE BRACCIO - 13 658 //pos_braccio4 = pos_braccio4 + 30;//non e' mirror quindi + 30 659 new_pos_braccio4 = 180 - pos_braccio4; 660 pulselengthzb = map(new_pos_braccio4, 0, 180, SERVOMIN_bra4, SERVOMAX_bra4); 661 pwm.setPWM(13, 0, pulselengthzb); 662 663 //SETTAGGIO INIZIALE SPALLA - 15 664 //new_pos_spalla4 = 180 - pos_spalla4; 665 new_pos_spalla4 = pos_spalla4; 666 pulselengthzc = map(new_pos_spalla4, 0, 180, SERVOMIN_spa4, SERVOMAX_spa4); 667 pwm.setPWM(15, 0, pulselengthzc); 668 669 670 671 672} 673//______________________________________________________________________________________________________ 674 675 int flag_memorizz_coord_bra1, flag_memorizz_coord_bra2, flag_memorizz_coord_bra3, flag_memorizz_coord_bra4; 676 677 int flag_memorizz_coord_avambra1, flag_memorizz_coord_avambra2, flag_memorizz_coord_avambra3, flag_memorizz_coord_avambra4; 678 679 int flag_memorizz_coord_zampa = 0; 680 681 int flag_fine_raddrizzamento1 = 0, flag_fine_raddrizzamento2 = 0, flag_fine_raddrizzamento3 = 0, flag_fine_raddrizzamento4 = 0; 682 683 684 685// VARIABILI ALTEZZA ROBOT 686int routine_altezza = 0; 687int step_altezza = 5; //di quanto deve diminuire altezza robot 688int flag_altezza = 0, flag_altezzaB = 0; //setup 689int fine_altezz_zampa1 = 0, fine_altezz_zampa2 = 0, fine_altezz_zampa3 = 0, fine_altezz_zampa4 = 0; 690int alt_posizione_bra1 , alt_posizione_bra2, alt_posizione_bra3, alt_posizione_bra4; 691int alt_posizione_avambra1, alt_posizione_avambra2, alt_posizione_avambra3, alt_posizione_avambra4; 692int flag_altezza_serial = 0; 693int pos_avambraccio1_alt_targ, pos_avambraccio2_alt_targ, pos_avambraccio3_alt_targ, pos_avambraccio4_alt_targ; 694int modo_altezza1 = 0, modo_altezza2 = 0, modo_altezza3 = 0, modo_altezza4 = 0; 695int timer_generico_equilibrio = 0; 696int flag_generico1 = 0; 697 698int pausa_reflex = 1; 699int speed_reflex = 1; //per il braccio sara' 1 mentre per l'avambr sara' speed_reflex * 2 700 701int cilla = 0; 702int equilibrio_off = 0; 703 704int crono_spinte = 0; 705int lentezza_spinte = 1; 706int no_mov_zampa1 = 0, no_mov_zampa2 = 0; 707int altezza_robot_totale = 0; 708int altezza_robot_zampa1 = 0, altezza_robot_zampa2 = 0, altezza_robot_zampa3 = 0, altezza_robot_zampa4 = 0; 709int timerk = 0; 710int flag_pippo = 0; 711int flag_incomingByte = 1; 712int flag_equilib_zampe = 0; 713 714//____________________________________________LOOP PRINCIPALE___________________________________________ 715void loop() 716{ 717 718 719 720 //ram_rimasta = freeRam(); 721 722 723 Serial.print(angolo_rotta_attuale); // rotta attuale 724 Serial.print(","); 725 Serial.print(angolo_rotta_target); // rotta target 726 Serial.print(","); 727 Serial.print(risposta_direzione); // risposta direzione 728 Serial.print(","); 729 Serial.print(movimento_on,DEC); //movimento on 730 Serial.print(","); 731 Serial.print(angolo_roll_attuale); // x roll 732 Serial.print(","); 733 Serial.print(angolo_pitch_attuale); // y pitch 734 Serial.print(","); 735 Serial.print(fase_mov_zampe,DEC); // fase_mov_zampe ---> fase movimento furtivo 736 Serial.print(","); 737 Serial.print(equilibrio_str); // equilibrio_str 738 Serial.print(","); 739 Serial.print(target_x); // target_x 740 Serial.print(","); 741 Serial.print(target_y); // target_y 742 Serial.print(","); 743 744 Serial.print(velocita_mov); // 745 Serial.print(","); 746 747 Serial.print(flag_kfin); // 748 Serial.print(","); 749 750 Serial.print(valore_vel_sin2_FIN); // 751 Serial.print(","); 752 753 Serial.print(valore_vel_sin4_FIN); // 754 Serial.print(","); 755 756 Serial.print(valore_vel_des1_FIN); // 757 Serial.print(","); 758 759 Serial.print(valore_vel_des3_FIN); // 760 Serial.print(","); 761 //---------------- 762 Serial.print(valore_tar_sin2_SOMMA); // era valore_tar_sin2_FIN = valore target per esempio 105 (posizione da raggiungere) 763 Serial.print(","); 764 Serial.print(valore_tar_sin4_SOMMA); // 765 Serial.print(","); 766 Serial.print(valore_tar_des1_SOMMA); // 767 Serial.print(","); 768 Serial.print(valore_tar_des3_SOMMA); // 769 Serial.print(","); 770 771 Serial.print(pos_braccio1_fin); // 772 Serial.print(","); 773 774 Serial.print(pos_avambraccio1_fin); // 775 Serial.print(","); 776 777 Serial.print(valore_tar_des1_FIN); // 778 Serial.print(","); 779 780 Serial.print(fsrReading1); // 781 Serial.print(","); 782 783 Serial.print(fsrReading2); // 784 Serial.print(","); 785 786 Serial.print(fsrReading3); // 787 Serial.print(","); 788 789 Serial.print(fsrReading4); // 790 Serial.print(","); 791 792 793 794 /* 795 Serial.print(fase_equilibrio); // 796 Serial.print(","); 797 798 Serial.print(timer_target_x_raggiunto); // 799 Serial.print(","); 800 801 Serial.print(angolo_roll_attuale); // 802 Serial.print(","); 803 */ 804 Serial.print(velocita_mov); // 805 Serial.print(","); 806 Serial.print(valore_tar_sin2); // 807 Serial.print(","); 808 Serial.print(dist_angolo_roll_con_target_x); // 809 Serial.print(","); 810 Serial.print(inclinazione_a); // 811 Serial.print(","); 812 813 Serial.print(fase_equilibrio); // 814 Serial.print(","); 815 Serial.print(timer_check_assi); // 816 Serial.print(","); 817 Serial.print(zona_velocita); // 818 Serial.print(","); 819 //Serial.print(ok_zampa2); // 820 //Serial.print(","); 821 822 Serial.print(array_vista[0]); 823 Serial.print(","); 824 Serial.print(array_vista[1]); 825 Serial.print(","); 826 Serial.print(array_vista[2]); 827 Serial.print(","); 828 Serial.print(array_vista[3]); 829 Serial.print(","); 830 Serial.print(array_vista[4]); 831 Serial.print(","); 832 Serial.print(array_vista[5]); 833 Serial.print(","); 834 Serial.print(array_vista[6]); 835 Serial.print(","); 836 Serial.print(array_vista[7]); 837 Serial.print(","); 838 Serial.print(array_vista[8]); 839 Serial.print(","); 840 Serial.print(array_vista[9]); 841 Serial.print(","); 842 843 Serial.print(array_vista[10]); 844 Serial.print(","); 845 Serial.print(array_vista[11]); 846 Serial.print(","); 847 Serial.print(array_vista[12]); 848 Serial.print(","); 849 Serial.print(array_vista[13]); 850 Serial.print(","); 851 Serial.print(array_vista[14]); 852 Serial.print(","); 853 Serial.print(array_vista[15]); 854 Serial.print(","); 855 Serial.print(array_vista[16]); 856 Serial.print(","); 857 Serial.print(array_vista[17]); 858 Serial.print(","); 859 Serial.print(array_vista[18]); 860 Serial.print(","); 861 Serial.print(array_vista[19]); //54a variabile spedita 862 Serial.print(","); 863Serial.print(ok_zampa3); 864Serial.print(","); 865Serial.print(pos_avambraccio1_fin); 866Serial.print(","); 867Serial.print(pos_braccio3_fin); 868Serial.print(","); 869Serial.print(pos_avambraccio3_fin); 870Serial.print(","); 871 872Serial.print(pos_braccio2_fin); 873Serial.print(","); 874Serial.print(pos_avambraccio2_fin); 875Serial.print(","); 876Serial.print(pos_braccio4_fin); 877Serial.print(","); 878Serial.print(pos_avambraccio4_fin); 879Serial.print(","); 880 881 882//Serial.print(","); 883 Serial.println(); 884 //Serial.print(ram_rimasta); 885 886 887 888 889 890 //---------------------------------------------------------------------------------- GYRO ENGINE 891 // if programming failed, don't try to do anything 892 893 if (!dmpReady) return; 894 // wait for MPU interrupt or extra packet(s) available 895 while (!mpuInterrupt && fifoCount < packetSize) { 896 897 898 // other program behavior stuff here 899 // . 900 // . 901 // . 902 // if you are really paranoid you can frequently test in between other 903 // stuff to see if mpuInterrupt is true, and if so, "break;" from the 904 // while() loop to immediately process the MPU data 905 // . 906 // . 907 // . 908 } 909 // reset interrupt flag and get INT_STATUS byte 910 mpuInterrupt = false; 911 mpuIntStatus = mpu.getIntStatus(); 912 // get current FIFO count 913 fifoCount = mpu.getFIFOCount(); 914 915 // check for overflow (this should never happen unless our code is too inefficient) 916 917 if ((mpuIntStatus & 0x10) || fifoCount == 1024) //1024 918 //if ((mpuIntStatus & 0x10) || fifoCount == 1024) //1024 919 { 920 // reset so we can continue cleanly 921 mpu.resetFIFO(); 922 923 //Serial.println(F("FIFO overflow!")); 924 // otherwise, check for DMP data ready interrupt (this should happen frequently) 925 } else if (mpuIntStatus & 0x02) 926 { 927 // wait for correct available data length, should be a VERY short wait 928 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 929 // read a packet from FIFO 930 mpu.getFIFOBytes(fifoBuffer, packetSize); 931 // track FIFO count here in case there is > 1 packet available 932 // (this lets us immediately read more without waiting for an interrupt) 933 fifoCount -= packetSize; 934 #ifdef OUTPUT_READABLE_YAWPITCHROLL 935 // display Euler angles in degrees 936 mpu.dmpGetQuaternion(&q, fifoBuffer); 937 mpu.dmpGetGravity(&gravity, &q); 938 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 939 z = ypr[0] * 180/M_PI; //yawn 940 x = ypr[1] * 270/M_PI; //roll SE SI RIDUCE IN 90/M_PI, I GRADI AUMENTANO MENO DURANTE INCLINAZIONE 941 y = ypr[2] * 270/M_PI; //pitch SE SI RIDUCE IN 90M_PI, I GRADI AUMENTANO MENO DURANTE INCLINAZIONE 942//--------------------------------------------- valore asse Y - PITCH 943 //angolo_pitch_reale = y; 944 angolo_pitch_attuale = y; 945//--------------------------------------------- valore asse X - LATERAL ROLL 946 //angolo_roll_reale = x; 947 angolo_roll_attuale = x; 948//--------------------------------------------- valore asse Z - YAWN BUSSOLA (trasformato in 360) 949 if (z <= 0 && z >= -179) 950 { 951 angolo_rotta_attuale = (z + 361) % 360; 952 } 953 if (z >= 0 && z <= 179) 954 { 955 angolo_rotta_attuale = (z + 360) % 360; 956 } 957 958 #endif 959}//fine else if (mpuIntStatus & 0x02) { 960 961 962 963 //------------------------------------------------- COMANDI DA TASTIERA E SERIALI DA PROCESSING 964 if (Serial.available() > 0) 965 { 966 incomingByte = Serial.read(); 967 if (incomingByte == 'p') 968 { 969 if (pausa == 0) 970 { 971 pausa = 1; 972 } 973 else 974 { 975 pausa = 0; 976 } 977 } 978 979 if (incomingByte == 'q') 980 { 981 flag_incomingByte = 0; 982 } //fine if (incomingByte == 'q') 983 984 985 if (flag_incomingByte == 0) 986 { 987 if (movimento_on == 0 || movimento_on == -1) 988 { 989 movimento_on = 1; 990 movimento_on_flag = 0; 991 flag_incomingByte = 1; 992 } 993 }// fine if (flag_incomingByte == 0) 994 995 if (flag_incomingByte == 0) 996 { 997 if (movimento_on == 1) 998 { 999 movimento_on = -1; 1000 movimento_on_flag = 0; 1001 flag_incomingByte == 1; 1002 } 1003 } 1004 1005 1006 1007 1008 1009 1010 1011 if (incomingByte == 'w') 1012 { 1013 reset_saltelli = 1; 1014 movimento_on = 0; // trotto sul posto 1015 movimento_on_flag = 0; 1016 } 1017 if (incomingByte == 'e') 1018 { 1019 movimento_on = -1; // stop movimenti 1020 movimento_on_flag = 0; 1021 } 1022 1023 1024 if (incomingByte == '+') 1025 { 1026 angolo_rotta_target = angolo_rotta_target +5; 1027 fase = 0; 1028 } 1029 if (incomingByte == '-') 1030 { 1031 angolo_rotta_target = angolo_rotta_target -5; 1032 fase = 0; 1033 } 1034 1035 if (incomingByte == 'z') { 1036 target_x = target_x +1;} 1037 if (incomingByte == 'x') { 1038 target_x = target_x -1;} 1039 1040 if (incomingByte == 'c') { 1041 target_y = target_y +1;} 1042 if (incomingByte == 'v') { 1043 target_y = target_y -1;} 1044 1045 1046 //----------------------------------------------------- 1047 //****************** ROBOT AUMENTO ALTEZZA 1048 if (incomingByte == 'o') //DEVI ESSERE IN MODALITA' 1 CAMMINATA SE NO NON FUNGE 1049 { 1050 pos_avambraccio1_alt_targ = pos_avambraccio1_alt_targ + 5; 1051 //flag_altezza_serial = 1; 1052 }// fine if (incomingByte == 'o') 1053 1054 //****************** ROBOT DIMINUZIONE ALTEZZA 1055 if (incomingByte == 'i') //DEVI ESSERE IN MODALITA' 1 CAMMINATA SE NO NON FUNGE 1056 { 1057 pos_avambraccio1_alt_targ = pos_avambraccio1_alt_targ - 5; 1058 //flag_altezza_serial = 2; 1059 }// fine if (incomingByte == 'o') 1060 1061 //****************** SET TARGET ALTEZZA ROBOT 1062 if (incomingByte == 'k') //DEVI ESSERE IN MODALITA' 1 CAMMINATA SE NO NON FUNGE 1063 { 1064 flag_altezza_serial = 1; 1065 }// fine if (incomingByte == 'k') 1066 1067 //----------------------------------------------------- 1068 1069 1070 1071 }//fine if (Serial.available() > 0) 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 //if (pausa != 1) 1082 //{ 1083 1084 1085 //------------------------------ SENSORI TATTILI ----------------------------------- 1086 // RALLENTA DI BRUTTO! 1087 fsrReading1 = analogRead(fsrAnalogPin1); 1088 fsrReading2 = analogRead(fsrAnalogPin2); 1089 fsrReading3 = analogRead(fsrAnalogPin3); 1090 fsrReading4 = analogRead(fsrAnalogPin4); 1091 //----------------------------------------------------------------------------------- 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141if (start == 10) 1142{ 1143 if (pillozzo == 0)//RICORDA CHE IL CODICE EQUILIBRIO E'DISABILITATO EH 1144 { 1145 ok_zampa3 = 0; 1146 moto_zampa3 = 1; 1147 1148 ok_zampa4 = 0; 1149 moto_zampa4 = 1; 1150 1151 pillozzo = 1; 1152 } 1153 1154 1155 1156 if (movimento_on == 1) 1157 { 1158 1159 1160 if(flag_altezza_serial == 1) 1161 { 1162 /* 1163 if(pillozzo == 0) 1164 { 1165 ok_zampa1 = 0; 1166 moto_zampa1 = 1; 1167 pillozzo = 1; 1168 //zampa1_no_equilibrio = 1; 1169 } 1170 */ 1171 ricarica_zampa3(); 1172 ricarica_zampa4(); 1173 } 1174 1175 1176 1177 1178 1179 1180 1181 /* 1182 movimento_zampa_new2(1,2,120); //avambraccio 1183 movimento_zampa_new2(2,1,70); //braccio 1184 1185 movimento_zampa_new4(1,2,110); //avambraccio 1186 movimento_zampa_new4(2,1,75); //braccio 1187 */ 1188 1189 //timerk = timerk + 1; 1190 //if (timerk == 5) 1191 //{ 1192 //ricarica_zampa4(); 1193 //timerk = 0; 1194 //} 1195 1196 1197 //lentezza_spinte = 3; 1198 //spinta_zampe_codice(); 1199 1200 1201 //proviamo a modificare la massima inclinazione del robot in fase di equilibrtio a 10 avambr e 20 braccio 1202 1203 /* 1204 timer_generico = timer_generico + 1; 1205 1206 //movimento_zampa_new2(2, 1, 20);//braccio 1207 1208 if(timer_generico <= 20) 1209 { 1210 movimento_zampa_new1(1, 1, 130);//avambraccio 1211 movimento_zampa_new1(2, 1, 20);//braccio 1212 1213 movimento_zampa_new2(1, 1, 130);//avambraccio 1214 movimento_zampa_new2(2, 1, 20);//braccio 1215 1216 movimento_zampa_new3(1, 1, 130);//avambraccio 1217 movimento_zampa_new3(2, 1, 20);//braccio 1218 1219 movimento_zampa_new4(1, 1, 130);//avambraccio 1220 movimento_zampa_new4(2, 1, 20);//braccio 1221 1222 } 1223 1224 if (timer_generico > 20 && timer_generico <= 30) //se dopo che l'avambr si e' fermato, il tasto flag_altezza_serial, l'avambr riprende fino a nuova destinazione 1225 { 1226 movimento_zampa_new1(1, 5, 60);//avambraccio 1227 movimento_zampa_new1(2, 3, 130);//braccio 1228 1229 movimento_zampa_new2(1, 5, 60);//avambraccio 1230 movimento_zampa_new2(2, 3, 130);//braccio 1231 1232 movimento_zampa_new3(1, 5, 60);//avambraccio 1233 movimento_zampa_new3(2, 3, 130);//braccio 1234 1235 movimento_zampa_new4(1, 5, 60);//avambraccio 1236 movimento_zampa_new4(2, 3, 130);//braccio 1237 } 1238 if (timer_generico > 30) 1239 { 1240 movimento_zampa_new1(1, 1, 130);//avambraccio 1241 movimento_zampa_new1(2, 5, 30);//braccio 1242 1243 movimento_zampa_new2(1, 1, 130);//avambraccio 1244 movimento_zampa_new2(2, 5, 30);//braccio 1245 1246 movimento_zampa_new3(1, 1, 130);//avambraccio 1247 movimento_zampa_new3(2, 5, 30);//braccio 1248 1249 movimento_zampa_new4(1, 1, 130);//avambraccio 1250 movimento_zampa_new4(2, 5, 30);//braccio 1251 } 1252 1253 */ 1254 1255 1256 1257 1258 // QUESTO E' L'ULTIMO CODICE GESTIONE SERVI! 8/1/2017 1259 //************************************************************************************** ZAMPA 1 SETUP 1260 //SETTAGGIO INIZIALE AVAMBRACCIO - 0 1261 new_pos_avambraccio1 = 180 - pos_avambraccio1_fin; 1262 pulselengthz = map(new_pos_avambraccio1, 0, 180, SERVOMIN_ava1, SERVOMAX_ava1); 1263 pwm.setPWM(0, 0, pulselengthz); 1264 1265 //SETTAGGIO INIZIALE BRACCIO - 1 1266 pulselengthzb = map(pos_braccio1_fin, 0, 180, SERVOMIN_bra1, SERVOMAX_bra1); 1267 pwm.setPWM(1, 0, pulselengthzb); 1268 1269 //SETTAGGIO INIZIALE SPALLA - 2 1270 new_pos_spalla1 = 180 - pos_spalla1_fin; 1271 pulselengthzc = map(new_pos_spalla1, 0, 180, SERVOMIN_spa1, SERVOMAX_spa1); 1272 pwm.setPWM(2, 0, pulselengthzc); 1273 1274 //************************************************************************************** ZAMPA 2 SETUP ---------- Con variabili fin! 1275 //SETTAGGIO INIZIALE AVAMBRACCIO - 4 1276 new_pos_avambraccio2 = 180 - pos_avambraccio2_fin; 1277 pulselengthz = map(new_pos_avambraccio2, 0, 180, SERVOMIN_ava2, SERVOMAX_ava2); 1278 pwm.setPWM(4, 0, pulselengthz); 1279 1280 //SETTAGGIO INIZIALE BRACCIO - 6 1281 new_pos_braccio2 = 180 - pos_braccio2_fin; 1282 pulselengthzb = map(new_pos_braccio2, 0, 180, SERVOMIN_bra2, SERVOMAX_bra2); 1283 pwm.setPWM(6, 0, pulselengthzb); 1284 1285 //SETTAGGIO INIZIALE SPALLA - 7 1286 new_pos_spalla2 = 180 - pos_spalla2_fin; 1287 pulselengthzc = map(new_pos_spalla2, 0, 180, SERVOMIN_spa2, SERVOMAX_spa2); 1288 pwm.setPWM(7, 0, pulselengthzc); 1289 1290 1291 //************************************************************************************** ZAMPA 3 SETUP 1292 //SETTAGGIO INIZIALE AVAMBRACCIO - 8 1293 pos_avambraccio3_fin = pos_avambraccio3_fin + 0;// e' mirror quindi + 10 1294 new_pos_avambraccio3 = 180 - pos_avambraccio3_fin; 1295 pulselengthz = map(new_pos_avambraccio3, 0, 180, SERVOMIN_ava3, SERVOMAX_ava3); 1296 pwm.setPWM(8, 0, pulselengthz); 1297 1298 //SETTAGGIO INIZIALE BRACCIO - 9 1299 pulselengthzb = map(pos_braccio3_fin, 0, 180, SERVOMIN_bra3, SERVOMAX_bra3); 1300 pwm.setPWM(9, 0, pulselengthzb); 1301 1302 //SETTAGGIO INIZIALE SPALLA - 10 1303 //new_pos_spalla3 = 180 - pos_spalla3_fin; 1304 //pulselengthzc = map(new_pos_spalla3, 0, 180, SERVOMIN_spa3, SERVOMAX_spa3); 1305 //pwm.setPWM(10, 0, pulselengthzc); 1306 1307 1308 //************************************************************************************** ZAMPA 4 SETUP 1309 //SETTAGGIO INIZIALE AVAMBRACCIO - 12 1310 new_pos_avambraccio4 = pos_avambraccio4_fin; 1311 pulselengthz = map(new_pos_avambraccio4, 0, 180, SERVOMIN_ava4, SERVOMAX_ava4); 1312 pwm.setPWM(12, 0, pulselengthz); 1313 1314 //SETTAGGIO INIZIALE BRACCIO - 13 1315 new_pos_braccio4 = 180 - pos_braccio4_fin; 1316 pulselengthzb = map(new_pos_braccio4, 0, 180, SERVOMIN_bra4, SERVOMAX_bra4); 1317 pwm.setPWM(13, 0, pulselengthzb); 1318 1319 //SETTAGGIO INIZIALE SPALLA - 15 1320 //new_pos_spalla4 = pos_spalla4_fin; 1321 //pulselengthzc = map(new_pos_spalla4, 0, 180, SERVOMIN_spa4, SERVOMAX_spa4); 1322 //pwm.setPWM(15, 0, pulselengthzc); 1323 1324 1325 1326 }//fine if (movimento_on == 1) 1327}// fine start == 10 1328 1329 1330 1331 1332 1333 1334 1335 1336 1337 1338 1339 1340 1341 /////////////////////////////////////////////////////////////////////////////////////////////////////// 1342 /////////////////////////////////////////////////////////////////////////////////////////////////////// 1343 /////////////////////////////////////////////////////////////////////////////////////////////////////// 1344 ////////////////////////////////////////// CAMMINATA TEST FURTIVA ///////////////////////////////////// 1345 /////////////////////////////////////////////////////////////////////////////////////////////////////// 1346 /////////////////////////////////////////////////////////////////////////////////////////////////////// 1347 /////////////////////////////////////////////////////////////////////////////////////////////////////// 1348 //---------------------------- ROUTINE EQUILIBRIO GIROSCOPIO --------------------- 1349 //jizzy si inclina a destra ---> x negativo 1350 //jizzy si inclina a sinistra ---> x positivo 1351 1352if (start == 7) 1353{ 1354 1355 secondi = millis() / 1000; 1356 if (secondi < 22) //DEVONO ESSERE 22 PER PERMETTERE AL GYRO DI CALIBRARSI! 1357 { 1358 risposta_direzione = "CALIBRAZIONE"; 1359 //Serial.println(F("************* CALIBRAZIONE GYRO ***************")); 1360 camminata_test = 1; 1361 } 1362 else 1363 { 1364 secondi = 0; 1365 risposta_direzione = "GYRO OK"; 1366 1367 1368if(movimento_on == 1) 1369{ 1370 //PULSANTE RIDUZIONE ALTEZZA ZAMPA (deve stare qui perche' il codice di movimento dei servi e' dentro movimento_on = 1 1371 //if (flag_altezza_serial == 1) 1372 //{ 1373 // step_altezza = 15; 1374 // riduzione_altezza_zampe(); 1375 //} 1376 1377 1378//********************************************************************************************* 1379//********************************************************************************************* 1380//********************************** ********************************** 1381//********************************* ROUTINE EQUILIBRIO GYRO ********************************* 1382//********************************** ********************************** 1383//********************************************************************************************* 1384//********************************************************************************************* 1385 1386 //LENTA 1387 if (angolo_roll_attuale <= target_x+4 || angolo_roll_attuale >= target_x-4 || angolo_pitch_attuale <= target_y+4 || angolo_pitch_attuale >= target_y-4) 1388 { 1389 zona_velocita = 1; //lenta 1390 //fase_equilibrio_B == 0; //quando siamo in zona_velocita' lenta, la fase dell'altra zona viene resettata! 1391 targ_crono_vel_equilib = 3; 1392 } 1393 1394 //MEDIA 1395 1396 1397 //VELOCE 1398 if (angolo_roll_attuale > target_x+4 || angolo_roll_attuale < target_x-4 || angolo_pitch_attuale > target_y+4 || angolo_pitch_attuale < target_y-4) 1399 { 1400 zona_velocita = 2; //2; //veloce 1401 //fase_equilibrio = 0; //quando siamo in zona_velocita' veloce, la fase dell'altra zona viene resettata! 1402 targ_crono_vel_equilib = 1; 1403 } 1404 1405 1406 1407//************************************************************************************************************************************ 1408//************************************************************************************************************************************ 1409//************************************ ESECUZIONE EQUILIBRIO ************************************************************************* 1410//************************************************************************************************************************************ 1411//************************************************************************************************************************************ 1412if(spinta_zampe == 0) 1413{ 1414 //---------------------------------------------------------------------- 1415 //----------------------------------------------------- ASSE X 1416 //---------------------------------------------------------------------- 1417 //ROBOT DRITTO 1418 if (angolo_roll_attuale == target_x) 1419 { 1420 //fermi 2 e 4 1421 valore_tar_sin2 = 0; 1422 valore_tar_sin4 = 0; 1423 //fermi 1 e 3 1424 valore_tar_des1 = 0; 1425 valore_tar_des3 = 0; 1426 } 1427 1428 //INCLINAZIONE A SINISTRA 1429 if (angolo_roll_attuale > target_x) 1430 { 1431 1432 //Apertura 2 e 4 1433 valore_tar_sin2 = 1; 1434 valore_tar_sin4 = 1; 1435 //Chiusura 1 e 3 1436 valore_tar_des1 = -1; 1437 valore_tar_des3 = -1; 1438 //fase_equilibrio = 1; 1439 } 1440 1441 //INCLINAZIONE A DESTRA 1442 if (angolo_roll_attuale < target_x) 1443 { 1444 //Apertura 2 e 4 1445 valore_tar_sin2 = -1; 1446 valore_tar_sin4 = -1; 1447 //Chiusura 1 e 3 1448 valore_tar_des1 = 1; 1449 valore_tar_des3 = 1; 1450 //fase_equilibrio = 1; 1451 } 1452 1453 //---------------------------------------------------------------------- 1454 //----------------------------------------------------- ASSE Y 1455 //---------------------------------------------------------------------- 1456 //ROBOT DRITTO 1457 if (angolo_pitch_attuale == target_y) 1458 { 1459 //fermi 1 e 2 1460 valore_tar_des1b = 0; 1461 valore_tar_sin2b = 0; 1462 //fermi 3 e 4 1463 valore_tar_des3b = 0; 1464 valore_tar_sin4b = 0; 1465 } 1466 1467 //INCLINAZIONE CABRATA 1468 if (angolo_pitch_attuale > target_y) 1469 { 1470 // 1 e 2 chiudono 1471 valore_tar_des1b = -1; 1472 valore_tar_sin2b = -1; 1473 //3 e 4 aprono 1474 valore_tar_des3b = 1; 1475 valore_tar_sin4b = 1; 1476 } 1477 1478 //INCLINAZIONE PICCHIATA 1479 if (angolo_pitch_attuale < target_y) 1480 { 1481 // 1 e 2 aprono 1482 valore_tar_des1b = 1; 1483 valore_tar_sin2b = 1; 1484 //3 e 4 chiudono 1485 valore_tar_des3b = -1; 1486 valore_tar_sin4b = -1; 1487 } 1488 1489 1490//------------------------- 1491 1492 1493 //SOMMIAMO I VALORI TAR IN AMBITO X con QUELLI IN AMBITO Y 1494 valore_tar_sin2_SOMMA = valore_tar_sin2 + valore_tar_sin2b; 1495 valore_tar_sin4_SOMMA = valore_tar_sin4 + valore_tar_sin4b; 1496 valore_tar_des1_SOMMA = valore_tar_des1 + valore_tar_des1b; 1497 valore_tar_des3_SOMMA = valore_tar_des3 + valore_tar_des3b; 1498 1499 1500 if (valore_tar_sin2_SOMMA > 1) 1501 { 1502 valore_tar_sin2_SOMMA = 1; 1503 } 1504 if (valore_tar_sin2_SOMMA < -1) 1505 { 1506 valore_tar_sin2_SOMMA = -1; 1507 } 1508 //----- 1509 if (valore_tar_sin4_SOMMA > 1) 1510 { 1511 valore_tar_sin4_SOMMA = 1; 1512 } 1513 if (valore_tar_sin4_SOMMA < -1) 1514 { 1515 valore_tar_sin4_SOMMA = -1; 1516 } 1517 //----- 1518 if (valore_tar_des1_SOMMA > 1) 1519 { 1520 valore_tar_des1_SOMMA = 1; 1521 } 1522 if (valore_tar_des1_SOMMA < -1) 1523 { 1524 valore_tar_des1_SOMMA = -1; 1525 } 1526 //----- 1527 if (valore_tar_des3_SOMMA > 1) 1528 { 1529 valore_tar_des3_SOMMA = 1; 1530 } 1531 if (valore_tar_des3_SOMMA < -1) 1532 { 1533 valore_tar_des3_SOMMA = -1; 1534 } 1535 1536 1537/* 1538 --------- riprova aggiungendo questo pezzo 1539 /////////////////////////////////////////////////////////////////////////////////////// 1540 // FASE 1 - SET TAR FINALE 1541 // 1542 /////////////////////////////////////////////////////////////////////////////////////// 1543 1544 //POSIZIONE DA RAGGIUNGERE PER OGNI AVAMBRACCIO, i valori "valore_tar_SOMMA" possono essere 1 o -1 quindi 1545 // l'avambraccio si chiude o si apre 1 pixel alla volta 1546 valore_tar_sin2_FIN = pos_avambraccio2_fin + valore_tar_sin2_SOMMA; 1547 valore_tar_sin4_FIN = pos_avambraccio4_fin + valore_tar_sin4_SOMMA; 1548 valore_tar_des1_FIN = pos_avambraccio1_fin + valore_tar_des1_SOMMA; 1549 valore_tar_des3_FIN = pos_avambraccio3_fin + valore_tar_des3_SOMMA; 1550 1551 //POSIZIONE MASSIMA RAGGIUNGIBILE DAGLI AVAMBRACCI 1552 //Zampe 2 e 4 avambraccia 1553 if(valore_tar_sin2_FIN > 120){valore_tar_sin2_FIN = 120;} 1554 if(valore_tar_sin2_FIN < 20){valore_tar_sin2_FIN = 20;} 1555 if(valore_tar_sin4_FIN > 110){valore_tar_sin4_FIN = 110;} 1556 if(valore_tar_sin4_FIN < 30){valore_tar_sin4_FIN = 30;} 1557 1558 //Zampe 1 e 3 avambraccia 1559 if(valore_tar_des1_FIN > 120){valore_tar_des1_FIN = 120;} 1560 if(valore_tar_des1_FIN < 20){valore_tar_des1_FIN = 20;} 1561 if(valore_tar_des3_FIN > 110){valore_tar_des3_FIN = 110;} 1562 if(valore_tar_des3_FIN < 30){valore_tar_des3_FIN = 30;} 1563------- 1564*/ 1565 1566 /////////////////////////////////////////////////////////////////////////////////////// 1567 // FASE 2 - MOVIMENTO ZAMPE 1568 // 1569 /////////////////////////////////////////////////////////////////////////////////////// 1570 //movimento_zampa_new1(int servo_num, int st_speed, int target) 1571 // 1 = avambr 2 1572 // 2 = bra 1 1573 // 3 = spa 1574 1575 1576 1577//se mi ritrovo nel counter ma parte la velocita' fast, eseguo senza aspettare piu' il counter! 1578timer_generico3 = timer_generico3 + 1; 1579if(timer_generico3 == targ_crono_vel_equilib || zona_velocita == 2) 1580{ 1581 //--------------------------------------------------------- ZAMPA 2 1582 if(zampa2_no_equilibrio == 0) 1583 { 1584 //ZAMPA 2 1585 if(valore_tar_sin2_SOMMA > 0) //la zampa si apre 1586 { 1587 movimento_zampa_new2(1,2,120); //avambr 1588 if (pos_braccio2_fin >= 70) 1589 { 1590 movimento_zampa_new2(2,1,70); //bra 1591 } 1592 } 1593 1594 if(valore_tar_sin2_SOMMA < 0) // la zampa si chiude 1595 { 1596 if(fsrReading2 > 250) 1597 { 1598 movimento_zampa_new2(1,2,20); //avambr 1599 if (pos_braccio2_fin >= 70) 1600 { 1601 movimento_zampa_new2(2,1,130); //bra 1602 } 1603 } 1604 else 1605 { 1606 /* 1607 //----------- LA ZAMPA 2 SI RIAPRE IN EMERGENZA PER RIATTACCARSI AL SUOLO! 1608 movimento_zampa_new2(1,2,120); //avambr 1609 if (pos_braccio2_fin >= 70) 1610 { 1611 movimento_zampa_new2(2,1,70); //bra 1612 } 1613 */ 1614 } 1615 1616 1617 } 1618 } 1619 1620 1621 //--------------------------------------------------------- ZAMPA 4 1622 if(zampa4_no_equilibrio == 0) 1623 { 1624 //ZAMPA 4 1625 if(valore_tar_sin4_SOMMA > 0) //zampa si apre 1626 { 1627 movimento_zampa_new4(1,2,110); //avambr 1628 if (pos_braccio4_fin < 80) 1629 { 1630 movimento_zampa_new4(2,1,75); //bra 1631 } 1632 1633 } // fine if(valore_tar_sin4_SOMMA > 0) 1634 1635 if(valore_tar_sin4_SOMMA < 0) //zampa si chiude 1636 { 1637 1638 if(fsrReading4 > 250) 1639 { 1640 movimento_zampa_new4(1,2,10); //avambr 1641 if (pos_braccio4_fin < 80) 1642 { 1643 movimento_zampa_new4(2,1,20); //bra 1644 } 1645 } 1646 else 1647 { 1648 /* 1649 //----------- LA ZAMPA 4 SI RIAPRE IN EMERGENZA PER RIATTACCARSI AL SUOLO! 1650 movimento_zampa_new4(1,2,110); //avambr 1651 if (pos_braccio4_fin < 80) 1652 { 1653 movimento_zampa_new4(2,1,75); //bra 1654 } 1655 */ 1656 } 1657 1658 } //fine if(valore_tar_sin4_SOMMA < 0) 1659 } 1660 1661 1662 //--------------------------------------------------------- ZAMPA 1 1663 if(zampa1_no_equilibrio == 0) 1664 { 1665 //ZAMPA 1 1666 if(valore_tar_des1_SOMMA > 0) 1667 { 1668 movimento_zampa_new1(1,2,120); //avambr 1669 if (pos_braccio1_fin >= 70) 1670 { 1671 movimento_zampa_new1(2,1,70); //bra 1672 } 1673 1674 } 1675 1676 if(valore_tar_des1_SOMMA < 0) 1677 { 1678 1679 if(fsrReading1 > 250) 1680 { 1681 movimento_zampa_new1(1,2,20); //avambr 1682 if (pos_braccio1_fin >= 70) 1683 { 1684 movimento_zampa_new1(2,1,130); //bra 1685 } 1686 } 1687 else 1688 { 1689 /* 1690 //----------- LA ZAMPA 1 SI RIAPRE IN EMERGENZA PER RIATTACCARSI AL SUOLO! 1691 movimento_zampa_new1(1,2,120); //avambr 1692 if (pos_braccio1_fin >= 70) 1693 { 1694 movimento_zampa_new1(2,1,70); //bra 1695 } 1696 */ 1697 } 1698 1699 } 1700 } 1701 1702 1703 //--------------------------------------------------------- ZAMPA 3 1704 if(zampa3_no_equilibrio == 0) 1705 { 1706 //ZAMPA 3 1707 if(valore_tar_des3_SOMMA > 0) //zampa si apre 1708 { 1709 movimento_zampa_new3(1,2,110); //avambr 1710 if (pos_braccio3_fin < 80) 1711 { 1712 movimento_zampa_new3(2,1,75); //bra 1713 } 1714 1715 }//fine if(valore_tar_des3_SOMMA > 0) 1716 1717 if(valore_tar_des3_SOMMA < 0) //zampa si chiude 1718 { 1719 1720 if(fsrReading3 > 250) 1721 { 1722 movimento_zampa_new3(1,2,10); //avambr 1723 if (pos_braccio3_fin < 80) 1724 { 1725 movimento_zampa_new3(2,1,20); //bra 1726 } 1727 } 1728 else 1729 { 1730 /* 1731 //----------- LA ZAMPA 3 SI RIAPRE IN EMERGENZA PER RIATTACCARSI AL SUOLO! 1732 movimento_zampa_new3(1,2,110); //avambr 1733 if (pos_braccio3_fin < 80) 1734 { 1735 movimento_zampa_new3(2,1,75); //bra 1736 } 1737 */ 1738 } 1739 1740 }//fine if(valore_tar_des3_SOMMA < 0) 1741 } 1742 1743timer_generico3 = 0; 1744}//fine if(timer_generico3 == 3) 1745 1746}//fine if(spinta_zampe == 0) 1747 1748if(spinta_zampe == 1) 1749{ 1750 spinta_zampe_codice(); 1751} 1752 1753//altezza_zampe(); 1754//questo altezza zampe che ci fa qui??? 1755 1756 1757 1758 1759//********************************************************************************************* 1760//********************************************************************************************* 1761//********************************************************************************************* 1762//********************************************************************************************* 1763//********************************** ******************************** 1764//********************************* ROUTINE CAMMINATA FURTIVA ******************************* 1765//********************************** ******************************** 1766//********************************************************************************************* 1767//********************************************************************************************* 1768//********************************************************************************************* 1769//********************************************************************************************* 1770//********************************************************************************************* 1771 1772/* 1773 equilib pre ricarica zampa 3 1774 prima raggiungere X 20 e poi Y picchiare -2 1775 1776 e verificare quanto indietro e' la zampa1 nel freeze di 1777 equilibrio pre ricarica! se fosse piu' indietro, sarebbe 1778 piu' semplice equilibrare la zampa 3? 1779*/ 1780 1781// FASE 0 ---------------------------------------------------------------------------------------------------- 1782//------------------------------- IL ROBOT SI STABILIZZA DRITTO PRIMA DI INIZIARE CAMMINATA ------------------ 1783//------------------------------------------------------------------------------------------------------------ 1784if (fase_mov_zampe == 0) 1785{ 1786 //riflessi_massimi = 0; 1787 durata_timer = 2; // si puo' metter 2! 1788 durata_spinte = 15; //era 15 1789 tempo_attesa_appo = 2; //velocit equilibrio_per_ricarica 1790/* 1791 target_x_appo = 20;// inclinaz a sinistra ** 15 ** 1792 target_y_appo = -5; //-5;// picchiata 1793 tempo_attesa_appo = 1; //serve per dire a che velocita' avverra' il raggiungimento di target_x_appo! piu' alta la variabile piu' lento il movimento 1794 equilibrio_per_ricarica(); 1795*/ 1796 1797 1798// timer_generico = timer_generico + 1; 1799// if(timer_generico >= durata_timer) 1800 1801//zampa1_no_equilibrio = 1; 1802//equilibrio pre ricarica zampa 1 1803//target X = 10 target Y = 5 con zampa 1 disabilitata! ed equilibra bene!!! 1804 1805 if(flag_altezza_serial == 1) 1806 { 1807 /* 1808 if(pillozzo == 0) 1809 { 1810 ok_zampa1 = 0; 1811 moto_zampa1 = 1; 1812 pillozzo = 1; 1813 //zampa1_no_equilibrio = 1; 1814 } 1815 */ 1816 //ricarica_zampa1(); 1817 1818 1819 lentezza_spinte = 3; 1820 spinta_zampe_codice(); 1821 } 1822 1823 1824 timer_generico2 = timer_generico2 + 1; 1825 1826 //ok provare i nuovi limiti minimi di abbassamento delle zampe anteriori (le posteriori in ambito minimo/massimo ora vanno bene!) 1827 1828 if(angolo_roll_attuale == target_x && angolo_pitch_attuale == target_y || timer_generico2 > 50) 1829 { 1830 timer_generico = timer_generico + 1; 1831 if(timer_generico >= durata_timer) // durata_timer) 1832 { 1833 1834 timer_generico = 0; 1835 fase_mov_zampe = 1;//------------------------------------------------------------------------------------------------------------QUI 1836 1837 1838 } 1839 } 1840 else 1841 { 1842 timer_generico = 0; 1843 } 1844} 1845 1846 1847 1848// FASE 1 ---------------------------------------------------------------------------------------------------- 1849//-------------------------------------------- TUTTE LE ZAMPE SPINGONO! 0 oppure FASE TEST GENERICI ---------- 1850//------------------------------------------------------------------------------------------------------------ 1851if (fase_mov_zampe == 1) 1852{ 1853/* 1854 zampa1_no_equilibrio = 1; 1855 zampa2_no_equilibrio = 1; 1856 zampa3_no_equilibrio = 1; 1857 zampa4_no_equilibrio = 1; 1858 1859 1860 altezza_zampe(); //220 e' l'altezza iniziale 1861 1862 //SE ALTEZZA ROBOT INFERIORE ALLA MEDIA (211 / 229) ALLORA IL ROBOT SI ALZA 1863 if(altezza_robot_totale <= 190) 1864 { 1865 modo_altezza1 = 1; //zampa si allunga 1866 modo_altezza2 = 1; //zampa si allunga 1867 modo_altezza3 = 1; //zampa si allunga 1868 modo_altezza4 = 1; //zampa si allunga 1869 } 1870 //SE ALTEZZA ROBOT SUPERIORE ALLA MEDIA (211 / 229) ALLORA IL ROBOT SI ABBASSA 1871 if(altezza_robot_totale >= 200) 1872 { 1873 modo_altezza1 = 2; //zampa si abbassa 1874 modo_altezza2 = 2; //zampa si abbassa 1875 modo_altezza3 = 2; //zampa si abbassa 1876 modo_altezza4 = 2; //zampa si abbassa 1877 } 1878 //SE ALTEZZA ROBOT RIENTRA NELL'AREA MEDIA (211 / 229) ALLORA IL ROBOT HA ESEGUITO IL RESET ALTEZZA! 1879 if(altezza_robot_totale > 190 && altezza_robot_totale < 200) 1880 { 1881 1882 modo_altezza1 = 0; //zampa si abbassa 1883 modo_altezza2 = 0; //zampa si abbassa 1884 modo_altezza3 = 0; //zampa si abbassa 1885 modo_altezza4 = 0; //zampa si abbassa 1886 1887 zampa1_no_equilibrio = 0; 1888 zampa2_no_equilibrio = 0; 1889 zampa3_no_equilibrio = 0; 1890 zampa4_no_equilibrio = 0; 1891 fase_mov_zampe = 2; 1892 } 1893*/ 1894 1895 1896 1897 1898 1899 fase_mov_zampe = 2; 1900 1901 1902} 1903 1904 1905// FASE 2 ---------------------------------------------------------------------------------------------------- 1906//---------------------------------- EQUILIBRA PER RICARICA ZAMPA 1 ---------------------------------- 1907//------------------------------------------------------------------------------------------------------------ 1908if (fase_mov_zampe == 2) 1909{ 1910/* 1911 //-------------------------------------------------------------- MEMORIZZIAMO COORDINATE ZAMPE 1912 if (flag_memorizz_coord_zampa == 0) 1913 { 1914 //flag_memorizz_coord_bra1 = pos_braccio1_fin; 1915 //flag_memorizz_coord_avambra1 = pos_avambraccio1_fin; 1916 flag_memorizz_coord_zampa = 1; 1917 } 1918*/ 1919 1920 if (flag_equilib_zampe == 0) 1921 { 1922 zampa1_no_equilibrio = 1; 1923 zampa2_no_equilibrio = 1; 1924 zampa3_no_equilibrio = 1; 1925 zampa4_no_equilibrio = 1; 1926 1927 altezza_zampe(); 1928 modo_altezza2 = 2; //zampa 2 si chiude 1929 modo_altezza4 = 2; //zampa 4 si chiude 1930 modo_altezza3 = 1; //zampa 3 si apre 1931 } 1932 1933 if (fsrReading1 <= 250 && angolo_roll_attuale >= 5) 1934 { 1935 flag_equilib_zampe = 1; 1936 1937 timer_generico = timer_generico + 1; 1938 if(timer_generico >= durata_timer) 1939 { 1940 zampa1_no_equilibrio = 0; 1941 zampa2_no_equilibrio = 0; 1942 zampa3_no_equilibrio = 0; 1943 zampa4_no_equilibrio = 0; 1944 1945 timer_generico = 0; 1946 timer_generico2 = 0; 1947 timer_generico_equilibrio = 0; 1948 timer_raggiung_target_x = 0; 1949 timer_raggiung_target_y = 0; 1950 flag_memorizz_coord_zampa = 0; 1951 flag_equilib_zampe = 0; 1952 fase_mov_zampe = 3; 1953 } 1954 } 1955 1956 1957/* 1958 //incliniamo il robot a sinistra, un grado alla volta fino che il piede e' bene schiacciato a terra 1959 if (fsrReading1 >= 250 || target_x < 5) 1960 { 1961 zampa1_no_equilibrio = 1; 1962 target_x_appo = target_x_appo + 1; 1963 equilibrio_per_ricarica(); 1964 } 1965 1966 //La pressione del piede a terra e' sotto la soglia e quindi FINE FASE 1967 if (fsrReading1 < 250 && target_x >= 5) 1968 { 1969 timer_generico = timer_generico + 1; 1970 if(timer_generico >= durata_timer) 1971 { 1972 timer_generico = 0; 1973 timer_generico2 = 0; 1974 timer_generico_equilibrio = 0; 1975 timer_raggiung_target_x = 0; 1976 timer_raggiung_target_y = 0; 1977 flag_memorizz_coord_zampa = 0; 1978 fase_mov_zampe = 3; 1979 } 1980 } 1981*/ 1982 1983 1984/* 1985 //********************************** NUOVA ROUTINE! 1986 //FINCHE' C'E' ABBASTANZA PRESSIONE NEL PIEDE, IL ROBOT SI INCLINA PER EQUILIBRARE. 1987 if (fsrReading1 > 250) 1988 { 1989 zampa1_no_equilibrio = 1; 1990 target_x_appo = 15; //10 20 10 1991 target_y_appo = 0; // 5 10 5 1992 //tempo_attesa_appo = 1; //serve per dire a che velocita' avverra' il raggiungimento di target_x_appo! piu' alta la variabile piu' lento il movimento 1993 equilibrio_per_ricarica(); 1994 } 1995 else 1996 { 1997 timer_generico = timer_generico + 1; 1998 if(timer_generico >= durata_timer) 1999 { 2000 2001 timer_generico = 0; 2002 timer_generico2 = 0; 2003 timer_generico_equilibrio = 0; 2004 timer_raggiung_target_x = 0; 2005 timer_raggiung_target_y = 0; 2006 flag_memorizz_coord_zampa = 0; 2007 fase_mov_zampe = 3; 2008 } 2009 }//fine else 2010*/ 2011 2012 2013 2014 2015 2016 2017} //fine if (fase_mov_zampe == 2) 2018 2019 2020 2021// FASE 3 ---------------------------------------------------------------------------------------------------- 2022//--------------------------------------------- RICARICA ZAMPA 1 --------------------------------------------- 2023//------------------------------------------------------------------------------------------------------------ 2024if (fase_mov_zampe == 3) 2025{ 2026 2027 zampa1_no_equilibrio = 1; // gia' settato a 1 nella fase precedente 2028 2029 // -------------------------------------------------------- CODICE DI SICUREZZA ANTI-BUG ZAMPA 1 2030 timer_generico = timer_generico + 1; 2031 if (timer_generico == 1) //prova con timer_generico == 0 2032 { 2033 ok_zampa1 = 0; 2034 moto_zampa1 = 1; 2035 } 2036 2037 // -------------------------------------------------------- MOVIMENTO RICARICA ZAMPA 1 2038 ricarica_zampa1(); 2039 2040 2041 // -------------------------------------------------------- FINE RICARICA ZAMPA 1 2042 if(ok_zampa1 == 1) 2043 { 2044 2045 2046 2047 // ----------------------------------- EQUILIBRIO RIABILITATO A ZAMPA 1 CHE HA FINITO DI CARICARE 2048 zampa1_no_equilibrio = 0; //la zampa 1, dopo la ricarica, riprende ad aiutare le altre zampe per equilibrio 2049 //zampa2_no_equilibrio = 0; 2050 //zampa3_no_equilibrio = 0; 2051 //zampa4_no_equilibrio = 0; 2052 2053 target_x_appo = 0;// inclinaz a des -2 2054 target_y_appo = 0; 2055 //tempo_attesa_appo = 1; //serve per dire a che velocita' avverra' il raggiungimento di target_x_appo! piu' alta la variabile piu' lento il movimento 2056 equilibrio_per_ricarica(); 2057/* 2058 // -------------------------------------------------------- RADRIZZAMENTO ZAMPE A FINE RICARICA ZAMPA 1 2059 if(flag_fine_raddrizzamento2 == 0) 2060 { 2061 zampa2_no_equilibrio = 1; 2062 2063 if(pos_avambraccio2_fin != flag_memorizz_coord_avambra2 && pos_braccio2_fin != flag_memorizz_coord_bra2) 2064 { 2065 movimento_zampa_new2(1 , 2, flag_memorizz_coord_avambra2); //avambr 20 2066 movimento_zampa_new2(2 , 1, flag_memorizz_coord_bra2); //bra 30 2067 } 2068 else 2069 { 2070 flag_fine_raddrizzamento2 = 1; 2071 zampa2_no_equilibrio = 0; 2072 } 2073 } //fine if(flag_fine_raddrizzamento2 == 0) 2074*/ 2075 2076 2077 // ----------------------------------- RAGGIUNTA L'INCLINAZIONE TARGET 0 2078 if(angolo_roll_attuale == target_x && angolo_pitch_attuale == target_y) // && pos_spalla1_fin == 70 && pos_spalla3_fin == 70 && pos_spalla2_fin == 50 && pos_spalla4_fin == 50) 2079 { 2080 timer_generico2 = timer_generico2 + 1; 2081 if(timer_generico2 >= durata_timer) 2082 { 2083 2084 zampa1_no_equilibrio = 0; //EQUILIBRIO RIABILITATO PER ZAMPA 1 2085 2086 2087 timer_generico = 0; 2088 timer_generico2 = 0; 2089 //timer_generico3 = 0; 2090 timer_generico_equilibrio = 0; 2091 timer_raggiung_target_x = 0; 2092 timer_raggiung_target_y = 0; 2093 flag_fine_raddrizzamento1 = 0, flag_fine_raddrizzamento2 = 0, flag_fine_raddrizzamento3 = 0, flag_fine_raddrizzamento4 = 0; 2094 fase_mov_zampe = 4; 2095 } 2096 } //fine if(target_x == angolo_roll_attuale && target_y == angolo_pitch_attuale) 2097 2098 }//fine if(ok_zampa1 == 1) 2099 2100 2101 2102} //fine if (fase_mov_zampe == 3) 2103 2104// FASE 4 ---------------------------------------------------------------------------------------------------- 2105//-------------------------------------------- RESET ALTEZZA ROBOT ------------------------------------------- 2106//------------------------------------------------------------------------------------------------------------ 2107if (fase_mov_zampe == 4) 2108{ 2109 2110 zampa1_no_equilibrio = 1; 2111 zampa2_no_equilibrio = 1; 2112 zampa3_no_equilibrio = 1; 2113 zampa4_no_equilibrio = 1; 2114 2115 2116 altezza_zampe(); //220 e' l'altezza iniziale 2117 2118 //SE ALTEZZA ROBOT INFERIORE ALLA MEDIA (211 / 229) ALLORA IL ROBOT SI ALZA 2119 if(altezza_robot_totale <= 230) //230 2120 { 2121 modo_altezza1 = 1; //zampa si allunga 2122 modo_altezza2 = 1; //zampa si allunga 2123 modo_altezza3 = 1; //zampa si allunga 2124 modo_altezza4 = 1; //zampa si allunga 2125 } 2126 //SE ALTEZZA ROBOT SUPERIORE ALLA MEDIA (211 / 229) ALLORA IL ROBOT SI ABBASSA 2127 if(altezza_robot_totale >= 240) //240 2128 { 2129 modo_altezza1 = 2; //zampa si abbassa 2130 modo_altezza2 = 2; //zampa si abbassa 2131 modo_altezza3 = 2; //zampa si abbassa 2132 modo_altezza4 = 2; //zampa si abbassa 2133 } 2134 //SE ALTEZZA ROBOT RIENTRA NELL'AREA MEDIA (211 / 229) ALLORA IL ROBOT HA ESEGUITO IL RESET ALTEZZA! 2135 if(altezza_robot_totale > 230 && altezza_robot_totale < 240) 2136 { 2137 2138 modo_altezza1 = 0; //zampa si abbassa 2139 modo_altezza2 = 0; //zampa si abbassa 2140 modo_altezza3 = 0; //zampa si abbassa 2141 modo_altezza4 = 0; //zampa si abbassa 2142 2143 zampa1_no_equilibrio = 0; 2144 zampa2_no_equilibrio = 0; 2145 zampa3_no_equilibrio = 0; 2146 zampa4_no_equilibrio = 0; 2147 fase_mov_zampe = 5; 2148 } 2149 2150 2151}// fine if (fase_mov_zampe == 4) 2152 2153 2154 2155// FASE 5 ---------------------------------------------------------------------------------------------------- 2156//-------------------------------------------- TUTTE LE ZAMPE SPINGONO! 1 -------------------------------------- 2157//------------------------------------------------------------------------------------------------------------ 2158if (fase_mov_zampe == 5) 2159{ 2160 2161 lentezza_spinte = 3; 2162 spinta_zampe = 1; 2163 2164 timer_generico = timer_generico + 1; 2165 if(timer_generico >= durata_spinte) 2166 { 2167 timer_generico2 = timer_generico2 + 1; 2168 if (timer_generico2 == durata_timer) 2169 { 2170 timer_generico = 0; 2171 timer_generico2 = 0; 2172 spinta_zampe = 0; 2173 fase_mov_zampe = 6; 2174 } 2175 } 2176 2177 2178 2179/* 2180 lentezza_spinte = 3; 2181 spinta_zampe = 1; 2182 2183 //timer_generico = timer_generico + 1; 2184 //if(timer_generico >= durata_spinte) 2185 if(pos_braccio2_fin >= 127 && pos_avambraccio2_fin >= 76) 2186 { 2187 spinta_zampe = 0; 2188 2189 timer_generico2 = timer_generico2 + 1; 2190 if (timer_generico2 == durata_timer) 2191 { 2192 timer_generico = 0; 2193 timer_generico2 = 0; 2194 fase_mov_zampe = 6; 2195 } 2196 } 2197*/ 2198 2199 2200 2201} 2202 2203 2204 2205// FASE 6 ---------------------------------------------------------------------------------------------------- 2206//---------------------------------- EQUILIBRA PER RICARICA ZAMPA 4 ------------------------------------------ 2207//------------------------------------------------------------------------------------------------------------ 2208if (fase_mov_zampe == 6) 2209{ 2210 2211 if (flag_equilib_zampe == 0) 2212 { 2213 zampa1_no_equilibrio = 1; 2214 zampa2_no_equilibrio = 1; 2215 zampa3_no_equilibrio = 1; 2216 zampa4_no_equilibrio = 1; 2217 2218 altezza_zampe(); 2219 modo_altezza1 = 2; //zampa 1 si chiude 2220 modo_altezza3 = 2; //zampa 3 si chiude 2221 modo_altezza2 = 1; //zampa 2 si apre 2222 } 2223 2224 if (fsrReading4 <= 250 && angolo_roll_attuale <= -5) 2225 { 2226 flag_equilib_zampe = 1; 2227 2228 timer_generico = timer_generico + 1; 2229 if(timer_generico >= durata_timer) 2230 { 2231 //zampa1_no_equilibrio = 0; 2232 //zampa2_no_equilibrio = 0; 2233 //zampa3_no_equilibrio = 0; 2234 //zampa4_no_equilibrio = 0; 2235 2236 timer_generico = 0; 2237 timer_generico2 = 0; 2238 timer_generico_equilibrio = 0; 2239 timer_raggiung_target_x = 0; 2240 timer_raggiung_target_y = 0; 2241 flag_memorizz_coord_zampa = 0; 2242 flag_equilib_zampe = 0; 2243 fase_mov_zampe = 7; 2244 } 2245 } 2246 2247 /* 2248 //incliniamo il robot a sinistra, un grado alla volta fino che il piede e' bene schiacciato a terra 2249 if (fsrReading4 >= 250 || target_x > -5) 2250 { 2251 zampa4_no_equilibrio = 1; 2252 target_x_appo = target_x_appo - 1; 2253 equilibrio_per_ricarica(); 2254 } 2255 2256 //La pressione del piede a terra e' sotto la soglia e quindi FINE FASE 2257 if (fsrReading4 < 250 && target_x <= -5) 2258 { 2259 timer_generico = timer_generico + 1; 2260 if(timer_generico >= durata_timer) 2261 { 2262 timer_generico = 0; 2263 timer_generico2 = 0; 2264 timer_generico_equilibrio = 0; 2265 timer_raggiung_target_x = 0; 2266 timer_raggiung_target_y = 0; 2267 flag_memorizz_coord_zampa = 0; 2268 fase_mov_zampe = 7; 2269 } 2270 } 2271*/ 2272 2273/* 2274 //********************************** NUOVA ROUTINE! 2275 //FINCHE' C'E' ABBASTANZA PRESSIONE NEL PIEDE, IL ROBOT SI INCLINA PER EQUILIBRARE. 2276 if (fsrReading4 > 250) 2277 { 2278 zampa4_no_equilibrio = 1; 2279 //era X=-5 e Y=0 2280 target_x_appo = -10; //10 -20 -5 -5 2281 target_y_appo = 2; // 5 -10 -5 0 2282 //tempo_attesa_appo = 2; //serve per dire a che velocita' avverra' il raggiungimento di target_x_appo! piu' alta la variabile piu' lento il movimento 2283 equilibrio_per_ricarica(); 2284 } 2285 else 2286 { 2287 2288 2289 timer_generico = timer_generico + 1; 2290 if(timer_generico >= durata_timer) 2291 { 2292 timer_generico = 0; 2293 timer_generico_equilibrio = 0; 2294 timer_raggiung_target_x = 0; 2295 timer_raggiung_target_y = 0; 2296 flag_memorizz_coord_zampa = 0; 2297 fase_mov_zampe = 7; 2298 } 2299 }//fine else 2300*/ 2301 2302 2303}//fine if (fase_mov_zampe == 6) 2304 2305 2306 2307 2308 2309 2310// FASE 7 ---------------------------------------------------------------------------------------------------- 2311//--------------------------------------------- RICARICA ZAMPA 4 --------------------------------------------- 2312//------------------------------------------------------------------------------------------------------------ 2313if (fase_mov_zampe == 7) 2314{ 2315 //zampa4_no_equilibrio = 1; // gia' settato a 1 nella fase precedente 2316 2317 // -------------------------------------------------------- CODICE DI SICUREZZA ANTI-BUG ZAMPA 4 2318 timer_generico = timer_generico + 1; 2319 if (timer_generico == 1) //prova con timer_generico == 0 2320 { 2321 ok_zampa4 = 0; 2322 moto_zampa4 = 1; 2323 } 2324 2325 // -------------------------------------------------------- MOVIMENTO RICARICA ZAMPA 4 2326 ricarica_zampa4(); 2327 2328 2329 // -------------------------------------------------------- FINE RICARICA ZAMPA 4 2330 if(ok_zampa4 == 1) 2331 { 2332 2333 // ----------------------------------- EQUILIBRIO RIABILITATO A ZAMPA 4 CHE HA FINITO DI CARICARE 2334 zampa4_no_equilibrio = 0; //la zampa 1, dopo la ricarica, riprende ad aiutare le altre zampe per equilibrio 2335 zampa1_no_equilibrio = 0; 2336 zampa2_no_equilibrio = 0; 2337 zampa3_no_equilibrio = 0; 2338 2339 target_x_appo = 0; //2 2340 target_y_appo = 0; 2341 //tempo_attesa_appo = 2; //serve per dire a che velocita' avverra' il raggiungimento di target_x_appo! piu' alta la variabile piu' lento il movimento 2342 equilibrio_per_ricarica(); 2343/* 2344 // -------------------------------------------------------- RADRIZZAMENTO ZAMPE A FINE RICARICA ZAMPA 1 2345 if(flag_fine_raddrizzamento2 == 0) 2346 { 2347 zampa2_no_equilibrio = 1; 2348 2349 if(pos_avambraccio2_fin != flag_memorizz_coord_avambra2 && pos_braccio2_fin != flag_memorizz_coord_bra2) 2350 { 2351 movimento_zampa_new2(1 , 2, flag_memorizz_coord_avambra2); //avambr 20 2352 movimento_zampa_new2(2 , 1, flag_memorizz_coord_bra2); //bra 30 2353 } 2354 else 2355 { 2356 flag_fine_raddrizzamento2 = 1; 2357 zampa2_no_equilibrio = 0; 2358 } 2359 } //fine if(flag_fine_raddrizzamento2 == 0) 2360*/ 2361 2362 2363 2364 2365 2366 // ----------------------------------- RAGGIUNTA L'INCLINAZIONE TARGET 0 2367 if(angolo_roll_attuale == target_x && angolo_pitch_attuale == target_y) // && pos_spalla1_fin == 70 && pos_spalla3_fin == 70 && pos_spalla2_fin == 50 && pos_spalla4_fin == 50) 2368 { 2369 timer_generico2 = timer_generico2 + 1; 2370 if(timer_generico2 >= durata_timer) 2371 { 2372 2373 zampa4_no_equilibrio = 0; //EQUILIBRIO RIABILITATO PER ZAMPA 4 2374 2375 2376 timer_generico = 0; 2377 timer_generico2 = 0; 2378 timer_generico3 = 0; 2379 timer_generico_equilibrio = 0; 2380 timer_raggiung_target_x = 0; 2381 timer_raggiung_target_y = 0; 2382 flag_fine_raddrizzamento1 = 0, flag_fine_raddrizzamento2 = 0, flag_fine_raddrizzamento3 = 0, flag_fine_raddrizzamento4 = 0; 2383 fase_mov_zampe = 8; 2384 } 2385 } //fine if(target_x == angolo_roll_attuale && target_y == angolo_pitch_attuale) 2386 2387 }//fine if(ok_zampa1 == 1) 2388} //fine if (fase_mov_zampe == 7) 2389 2390// FASE 8 ---------------------------------------------------------------------------------------------------- 2391//-------------------------------------------- RESET ALTEZZA ROBOT -------------------------------------------- 2392//------------------------------------------------------------------------------------------------------------- 2393if (fase_mov_zampe == 8) 2394{ 2395 2396 zampa1_no_equilibrio = 1; 2397 zampa2_no_equilibrio = 1; 2398 zampa3_no_equilibrio = 1; 2399 zampa4_no_equilibrio = 1; 2400 2401 2402 altezza_zampe(); //220 e' l'altezza iniziale 2403 2404 //SE ALTEZZA ROBOT INFERIORE ALLA MEDIA (211 / 229) ALLORA IL ROBOT SI ALZA 2405 if(altezza_robot_totale <= 230) 2406 { 2407 modo_altezza1 = 1; //zampa si allunga 2408 modo_altezza2 = 1; //zampa si allunga 2409 modo_altezza3 = 1; //zampa si allunga 2410 modo_altezza4 = 1; //zampa si allunga 2411 } 2412 //SE ALTEZZA ROBOT SUPERIORE ALLA MEDIA (211 / 229) ALLORA IL ROBOT SI ABBASSA 2413 if(altezza_robot_totale >= 240) 2414 { 2415 modo_altezza1 = 2; //zampa si abbassa 2416 modo_altezza2 = 2; //zampa si abbassa 2417 modo_altezza3 = 2; //zampa si abbassa 2418 modo_altezza4 = 2; //zampa si abbassa 2419 } 2420 //SE ALTEZZA ROBOT RIENTRA NELL'AREA MEDIA (211 / 229) ALLORA IL ROBOT HA ESEGUITO IL RESET ALTEZZA! 2421 if(altezza_robot_totale > 230 && altezza_robot_totale < 240) 2422 { 2423 2424 modo_altezza1 = 0; //zampa si abbassa 2425 modo_altezza2 = 0; //zampa si abbassa 2426 modo_altezza3 = 0; //zampa si abbassa 2427 modo_altezza4 = 0; //zampa si abbassa 2428 2429 zampa1_no_equilibrio = 0; 2430 zampa2_no_equilibrio = 0; 2431 zampa3_no_equilibrio = 0; 2432 zampa4_no_equilibrio = 0; 2433 fase_mov_zampe = 9; 2434 } 2435} 2436 2437// FASE 9 ---------------------------------------------------------------------------------------------------- 2438//-------------------------------------------- TUTTE LE ZAMPE SPINGONO! 2 -------------------------------------- 2439//------------------------------------------------------------------------------------------------------------ 2440if (fase_mov_zampe == 9) 2441{ 2442 2443 //riflessi_massimi = 1; 2444 //velocita_spinta = 1; //piu alto = piu' lento 2445 2446 lentezza_spinte = 3; 2447 spinta_zampe = 1; 2448 2449 timer_generico = timer_generico + 1; 2450 if(timer_generico >= durata_spinte) 2451 { 2452 timer_generico2 = timer_generico2 + 1; 2453 if (timer_generico2 == durata_timer) 2454 { 2455 timer_generico = 0; 2456 timer_generico2 = 0; 2457 spinta_zampe = 0; 2458 fase_mov_zampe = 10; 2459 } 2460 } 2461} 2462 2463 2464// FASE 10 --------------------------------------------------------------------------------------------------- 2465//-------------------------------------------EQUILIBRA PER RICARICA ZAMPA 2 ---------------------------------- 2466//------------------------------------------------------------------------------------------------------------ 2467if (fase_mov_zampe == 10) 2468{ 2469 2470 if (flag_equilib_zampe == 0) 2471 { 2472 zampa1_no_equilibrio = 1; 2473 zampa2_no_equilibrio = 1; 2474 zampa3_no_equilibrio = 1; 2475 zampa4_no_equilibrio = 1; 2476 2477 altezza_zampe(); 2478 modo_altezza1 = 2; //zampa 1 si chiude 2479 modo_altezza3 = 2; //zampa 3 si chiude 2480 modo_altezza4 = 1; //zampa 4 si apre 2481 } 2482 2483 if (fsrReading2 <= 250 && angolo_roll_attuale <= -5) 2484 { 2485 flag_equilib_zampe = 1; 2486 2487 timer_generico = timer_generico + 1; 2488 if(timer_generico >= durata_timer) 2489 { 2490 zampa1_no_equilibrio = 0; 2491 zampa2_no_equilibrio = 0; 2492 zampa3_no_equilibrio = 0; 2493 zampa4_no_equilibrio = 0; 2494 2495 timer_generico = 0; 2496 timer_generico2 = 0; 2497 timer_generico_equilibrio = 0; 2498 timer_raggiung_target_x = 0; 2499 timer_raggiung_target_y = 0; 2500 flag_memorizz_coord_zampa = 0; 2501 flag_equilib_zampe = 0; 2502 fase_mov_zampe = 11; 2503 } 2504 } 2505 2506 /* 2507 //incliniamo il robot a destra, un grado alla volta fino che il piede e' bene schiacciato a terra 2508 if (fsrReading2 >= 250 || target_x > -5) 2509 { 2510 zampa2_no_equilibrio = 1; 2511 target_x_appo = target_x_appo - 1; 2512 equilibrio_per_ricarica(); 2513 } 2514 2515 //La pressione del piede a terra e' sotto la soglia e quindi FINE FASE 2516 if (fsrReading2 < 250 && target_x <= -5) 2517 { 2518 timer_generico = timer_generico + 1; 2519 if(timer_generico >= durata_timer) 2520 { 2521 timer_generico = 0; 2522 timer_generico2 = 0; 2523 timer_generico_equilibrio = 0; 2524 timer_raggiung_target_x = 0; 2525 timer_raggiung_target_y = 0; 2526 flag_memorizz_coord_zampa = 0; 2527 fase_mov_zampe = 11; 2528 } 2529 } 2530 */ 2531 2532/* 2533 //********************************** NUOVA ROUTINE! 2534 //FINCHE' C'E' ABBASTANZA PRESSIONE NEL PIEDE, IL ROBOT SI INCLINA PER EQUILIBRARE. 2535 if (fsrReading2 > 250) 2536 { 2537 zampa2_no_equilibrio = 1; 2538 //era X=-5 e Y=5 2539 target_x_appo = -20; //10 20 2540 target_y_appo = 10; // 5 10 2541 //tempo_attesa_appo = 2; //serve per dire a che velocita' avverra' il raggiungimento di target_x_appo! piu' alta la variabile piu' lento il movimento 2542 equilibrio_per_ricarica(); 2543 } 2544 else 2545 { 2546 timer_generico = timer_generico + 1; 2547 if(timer_generico >= durata_timer) 2548 { 2549 timer_generico = 0; 2550 timer_generico_equilibrio = 0; 2551 timer_raggiung_target_x = 0; 2552 timer_raggiung_target_y = 0; 2553 flag_memorizz_coord_zampa = 0; 2554 fase_mov_zampe = 11; 2555 } 2556 }//fine else 2557*/ 2558 2559 2560} //fine if (fase_mov_zampe == 10) 2561 2562 2563 2564 2565// FASE 11 ---------------------------------------------------------------------------------------------------- 2566//--------------------------------------------- RICARICA ZAMPA 2 --------------------------------------------- 2567//------------------------------------------------------------------------------------------------------------ 2568if (fase_mov_zampe == 11) 2569{ 2570 zampa2_no_equilibrio = 1; // gia' settato a 1 nella fase precedente 2571 2572 // -------------------------------------------------------- CODICE DI SICUREZZA ANTI-BUG ZAMPA 2 2573 timer_generico = timer_generico + 1; 2574 if (timer_generico == 1) //prova con timer_generico == 0 2575 { 2576 ok_zampa2 = 0; 2577 moto_zampa2 = 1; 2578 } 2579 2580 // -------------------------------------------------------- MOVIMENTO RICARICA ZAMPA 2 2581 ricarica_zampa2(); 2582 2583 2584 // -------------------------------------------------------- FINE RICARICA ZAMPA 2 2585 if(ok_zampa2 == 1) 2586 { 2587 2588 // ----------------------------------- EQUILIBRIO RIABILITATO A ZAMPA 1 CHE HA FINITO DI CARICARE 2589 zampa2_no_equilibrio = 0; //la zampa 1, dopo la ricarica, riprende ad aiutare le altre zampe per equilibrio 2590 //zampa1_no_equilibrio = 0; 2591 //zampa3_no_equilibrio = 0; 2592 //zampa4_no_equilibrio = 0; 2593 2594 target_x_appo = 0;//2 2595 target_y_appo = 0; 2596 //tempo_attesa_appo = 2; //serve per dire a che velocita' avverra' il raggiungimento di target_x_appo! piu' alta la variabile piu' lento il movimento 2597 equilibrio_per_ricarica(); 2598/* 2599 // -------------------------------------------------------- RADRIZZAMENTO ZAMPE A FINE RICARICA ZAMPA 2 2600 if(flag_fine_raddrizzamento2 == 0) 2601 { 2602 zampa2_no_equilibrio = 1; 2603 2604 if(pos_avambraccio2_fin != flag_memorizz_coord_avambra2 && pos_braccio2_fin != flag_memorizz_coord_bra2) 2605 { 2606 movimento_zampa_new2(1 , 2, flag_memorizz_coord_avambra2); //avambr 20 2607 movimento_zampa_new2(2 , 1, flag_memorizz_coord_bra2); //bra 30 2608 } 2609 else 2610 { 2611 flag_fine_raddrizzamento2 = 1; 2612 zampa2_no_equilibrio = 0; 2613 } 2614 } //fine if(flag_fine_raddrizzamento2 == 0) 2615*/ 2616 2617 2618 // ----------------------------------- RAGGIUNTA L'INCLINAZIONE TARGET 0 2619 if(angolo_roll_attuale == target_x && angolo_pitch_attuale == target_y) // && pos_spalla1_fin == 70 && pos_spalla3_fin == 70 && pos_spalla2_fin == 50 && pos_spalla4_fin == 50) 2620 { 2621 timer_generico2 = timer_generico2 + 1; 2622 if(timer_generico2 >= durata_timer) 2623 { 2624 2625 zampa2_no_equilibrio = 0; //EQUILIBRIO RIABILITATO PER ZAMPA 2 2626 2627 2628 timer_generico = 0; 2629 timer_generico2 = 0; 2630 //timer_generico3 = 0; 2631 timer_generico_equilibrio = 0; 2632 timer_raggiung_target_x = 0; 2633 timer_raggiung_target_y = 0; 2634 flag_fine_raddrizzamento1 = 0, flag_fine_raddrizzamento2 = 0, flag_fine_raddrizzamento3 = 0, flag_fine_raddrizzamento4 = 0; 2635 fase_mov_zampe = 12; 2636 } 2637 } //fine if(target_x == angolo_roll_attuale && target_y == angolo_pitch_attuale) 2638 2639 }//fine if(ok_zampa2 == 1) 2640} 2641 2642 2643 2644// FASE 12 ---------------------------------------------------------------------------------------------------- 2645//-------------------------------------------- RESET ALTEZZA ROBOT -------------------------------------------- 2646//------------------------------------------------------------------------------------------------------------- 2647if (fase_mov_zampe == 12) 2648{ 2649 2650 zampa1_no_equilibrio = 1; 2651 zampa2_no_equilibrio = 1; 2652 zampa3_no_equilibrio = 1; 2653 zampa4_no_equilibrio = 1; 2654 2655 2656 altezza_zampe(); //220 e' l'altezza iniziale 2657 2658 //SE ALTEZZA ROBOT INFERIORE ALLA MEDIA (211 / 229) ALLORA IL ROBOT SI ALZA 2659 if(altezza_robot_totale <= 230) 2660 { 2661 modo_altezza1 = 1; //zampa si allunga 2662 modo_altezza2 = 1; //zampa si allunga 2663 modo_altezza3 = 1; //zampa si allunga 2664 modo_altezza4 = 1; //zampa si allunga 2665 } 2666 //SE ALTEZZA ROBOT SUPERIORE ALLA MEDIA (211 / 229) ALLORA IL ROBOT SI ABBASSA 2667 if(altezza_robot_totale >= 240) 2668 { 2669 modo_altezza1 = 2; //zampa si abbassa 2670 modo_altezza2 = 2; //zampa si abbassa 2671 modo_altezza3 = 2; //zampa si abbassa 2672 modo_altezza4 = 2; //zampa si abbassa 2673 } 2674 //SE ALTEZZA ROBOT RIENTRA NELL'AREA MEDIA (211 / 229) ALLORA IL ROBOT HA ESEGUITO IL RESET ALTEZZA! 2675 if(altezza_robot_totale > 230 && altezza_robot_totale < 240) 2676 { 2677 2678 modo_altezza1 = 0; //zampa si abbassa 2679 modo_altezza2 = 0; //zampa si abbassa 2680 modo_altezza3 = 0; //zampa si abbassa 2681 modo_altezza4 = 0; //zampa si abbassa 2682 2683 zampa1_no_equilibrio = 0; 2684 zampa2_no_equilibrio = 0; 2685 zampa3_no_equilibrio = 0; 2686 zampa4_no_equilibrio = 0; 2687 fase_mov_zampe = 13; 2688 } 2689} 2690 2691 2692 2693// FASE 13 ---------------------------------------------------------------------------------------------------- 2694//-------------------------------------------- TUTTE LE ZAMPE SPINGONO! 3 -------------------------------------- 2695//------------------------------------------------------------------------------------------------------------ 2696if (fase_mov_zampe == 13) 2697{ 2698 2699 2700 //durata_spinte = 25; 2701 2702 lentezza_spinte = 3; 2703 spinta_zampe = 1; 2704 2705 timer_generico = timer_generico + 1; 2706 if(timer_generico >= durata_spinte) 2707 { 2708 timer_generico2 = timer_generico2 + 1; 2709 if (timer_generico2 == durata_timer) 2710 { 2711 timer_generico = 0; 2712 timer_generico2 = 0; 2713 spinta_zampe = 0; 2714 fase_mov_zampe = 14; 2715 //durata_spinte = 15; 2716 } 2717 } 2718 2719 /* 2720 //bra1 120 2721 //ava1 64 2722 2723 lentezza_spinte = 3; 2724 spinta_zampe = 1; 2725 2726 //timer_generico = timer_generico + 1; 2727 //if(timer_generico >= durata_spinte) 2728 2729 if(pos_braccio1_fin >= 120 && pos_avambraccio1_fin >= 64) 2730 { 2731 spinta_zampe = 0; 2732 2733 timer_generico2 = timer_generico2 + 1; 2734 if (timer_generico2 == durata_timer) 2735 { 2736 timer_generico = 0; 2737 timer_generico2 = 0; 2738 fase_mov_zampe = 14; 2739 } 2740 } 2741 */ 2742 2743 2744 2745} 2746 2747 2748//e verifica se per ric zampa 3 il robot si inclina a sinistra e picchiata 2749 2750 2751/* 2752fare routine che fa durare spinta a seconda di posizione zampe! 2753 2754EQUILIBRIO RICARICA ZAMPA 1 2755RICARICA ZAMPA 1 2756RESET ALTEZZA 2757SPINTA 1 (dopo ricarica zampa 1) = La spinta finisce quando ZAMPA 2 raggiunge 2758 2759EQUILIBRIO RICARICA ZAMPA 4 2760RICARICA ZAMPA 4 2761RESET ALTEZZA 2762SPINTA 2 2763 2764EQUILIBRIO RICARICA ZAMPA 2 2765RICARICA ZAMPA 2 2766RESET ALTEZZA 2767SPINTA 3 (dopo ricarica zampa 2) = La spinta finisce quando ZAMPA 1 raggiunge 2768 2769EQUILIBRIO RICARICA ZAMPA 3 2770RICARICA ZAMPA 3 2771RESET ALTEZZA 2772SPINTA 4 2773 2774*/ 2775 2776 2777 2778 2779// FASE 14 --------------------------------------------------------------------------------------------------- 2780//---------------------------------- EQUILIBRA PER RICARICA ZAMPA 3 ------------------------------------------ 2781//------------------------------------------------------------------------------------------------------------ 2782if (fase_mov_zampe == 14) 2783{ 2784 2785 if (flag_equilib_zampe == 0) 2786 { 2787 zampa1_no_equilibrio = 1; 2788 zampa2_no_equilibrio = 1; 2789 zampa3_no_equilibrio = 1; 2790 zampa4_no_equilibrio = 1; 2791 2792 altezza_zampe(); 2793 modo_altezza2 = 2; //zampa 2 si chiude 2794 modo_altezza4 = 2; //zampa 4 si chiude 2795 modo_altezza1 = 1; //zampa 1 si apre 2796 2797 } 2798 2799 if (fsrReading3 <= 250 && angolo_roll_attuale >= 15)//15 2800 { 2801 flag_equilib_zampe = 1; 2802 2803 timer_generico = timer_generico + 1; 2804 if(timer_generico >= durata_timer) 2805 { 2806 //zampa1_no_equilibrio = 0; 2807 //zampa2_no_equilibrio = 0; 2808 //zampa3_no_equilibrio = 0; 2809 //zampa4_no_equilibrio = 0; 2810 2811 timer_generico = 0; 2812 timer_generico2 = 0; 2813 timer_generico_equilibrio = 0; 2814 timer_raggiung_target_x = 0; 2815 timer_raggiung_target_y = 0; 2816 flag_memorizz_coord_zampa = 0; 2817 flag_equilib_zampe = 0; 2818 fase_mov_zampe = 15; 2819 } 2820 } 2821 2822 2823/* 2824 if (flag_equilib_zampe == 0) 2825 { 2826 zampa1_no_equilibrio = 1; 2827 zampa2_no_equilibrio = 1; 2828 zampa3_no_equilibrio = 1; 2829 zampa4_no_equilibrio = 1; 2830 2831 altezza_zampe(); 2832 modo_altezza1 = 1; //zampa 1 si apre 2833 if(pos_avambraccio2_fin >= 40)//65 2834 { 2835 modo_altezza3 = 1; //zampa 1 si apre 2836 } 2837 else 2838 { 2839 modo_altezza3 = 0; //zampa 1 si ferma 2840 } 2841 2842 modo_altezza2 = 2; //zampa 2 si chiude 2843 modo_altezza4 = 2; //zampa 4 si chiude 2844 2845 //timer_generico2 = timer_generico2 + 1; 2846 //if(pos_avambraccio1_fin >= 90) 2847 //{ 2848 // modo_altezza2 = 2; //zampa 2 si chiude 2849 // modo_altezza4 = 2; //zampa 4 si chiude 2850 //} 2851 } 2852 2853 2854 if (fsrReading3 <= 250 && angolo_roll_attuale >= 5)//15 2855 { 2856 flag_equilib_zampe = 1; 2857 2858 timer_generico = timer_generico + 1; 2859 if(timer_generico >= durata_timer) 2860 { 2861 zampa1_no_equilibrio = 0; 2862 zampa2_no_equilibrio = 0; 2863 zampa3_no_equilibrio = 0; 2864 zampa4_no_equilibrio = 0; 2865 2866 timer_generico = 0; 2867 timer_generico2 = 0; 2868 timer_generico_equilibrio = 0; 2869 timer_raggiung_target_x = 0; 2870 timer_raggiung_target_y = 0; 2871 flag_memorizz_coord_zampa = 0; 2872 flag_equilib_zampe = 0; 2873 fase_mov_zampe = 15; 2874 } 2875 } 2876*/ 2877 2878 2879 2880 /* 2881 //incliniamo il robot a destra, un grado alla volta fino che il piede e' bene schiacciato a terra 2882 if (fsrReading3 >= 250 || target_x < 5) 2883 { 2884 zampa3_no_equilibrio = 1; 2885 target_x_appo = target_x_appo + 1; 2886 target_y_appo = 2; 2887 equilibrio_per_ricarica(); 2888 } 2889 2890 //La pressione del piede a terra e' sotto la soglia e quindi FINE FASE 2891 if (fsrReading3 < 250 && target_x >= 5) 2892 { 2893 timer_generico = timer_generico + 1; 2894 if(timer_generico >= durata_timer) 2895 { 2896 timer_generico = 0; 2897 timer_generico2 = 0; 2898 timer_generico_equilibrio = 0; 2899 timer_raggiung_target_x = 0; 2900 timer_raggiung_target_y = 0; 2901 flag_memorizz_coord_zampa = 0; 2902 fase_mov_zampe = 15; 2903 } 2904 } 2905 */ 2906 2907 2908 2909 2910 /* 2911 2912 zampa3_no_equilibrio = 1; 2913 target_x_appo = 10; 2914 target_y_appo = -5; 2915 equilibrio_per_ricarica(); 2916 2917 //incliniamo il robot a sinistra, un grado alla volta fino che il piede e' bene schiacciato a terra 2918 if (fsrReading3 >= 250) 2919 { 2920 //zampa3_no_equilibrio = 1; 2921 //target_x_appo = target_x_appo + 1; //usiamo + 2 per far andare piu' rapida l'inclinazione per evitare che sia troppo poca considerando che il sensore tattile 3 e' da verificare se funge bene quando la zampa 3 e' troppo indietro! 2922 //target_y_appo = -2; 2923 //equilibrio_per_ricarica(); 2924 } 2925 2926 //La pressione del piede a terra e' sotto la soglia e quindi FINE FASE 2927 if (fsrReading3 < 250) 2928 { 2929 timer_generico = timer_generico + 1; 2930 if(timer_generico >= durata_timer) 2931 { 2932 timer_generico = 0; 2933 timer_generico2 = 0; 2934 timer_generico_equilibrio = 0; 2935 timer_raggiung_target_x = 0; 2936 timer_raggiung_target_y = 0; 2937 flag_memorizz_coord_zampa = 0; 2938 fase_mov_zampe = 15; 2939 } 2940 } 2941 2942*/ 2943 2944 2945 2946 2947 2948 2949}//fine if (fase_mov_zampe == 14) 2950 2951 2952 2953// FASE 15 --------------------------------------------------------------------------------------------------- 2954//--------------------------------------------- RICARICA ZAMPA 3 --------------------------------------------- 2955//------------------------------------------------------------------------------------------------------------ 2956if (fase_mov_zampe == 15) 2957{ 2958 2959 //zampa3_no_equilibrio = 1; // gia' settato a 1 nella fase precedente 2960 2961 2962 // -------------------------------------------------------- CODICE DI SICUREZZA ANTI-BUG ZAMPA 3 2963 timer_generico = timer_generico + 1; 2964 if (timer_generico == 1) //prova con timer_generico == 0 2965 { 2966 ok_zampa3 = 0; 2967 moto_zampa3 = 1; 2968 } 2969 2970 // -------------------------------------------------------- MOVIMENTO RICARICA ZAMPA 3 2971 ricarica_zampa3(); 2972 2973 2974 2975 // -------------------------------------------------------- FINE RICARICA ZAMPA 3 2976 if(ok_zampa3 == 1) 2977 { 2978 2979 // ----------------------------------- EQUILIBRIO RIABILITATO A ZAMPA 3 CHE HA FINITO DI CARICARE 2980 zampa3_no_equilibrio = 0; //la zampa 1, dopo la ricarica, riprende ad aiutare le altre zampe per equilibrio 2981 zampa1_no_equilibrio = 0; 2982 zampa2_no_equilibrio = 0; 2983 zampa4_no_equilibrio = 0; 2984 2985 target_x_appo = 0;//-2 2986 target_y_appo = 0; 2987 //tempo_attesa_appo = 2; //serve per dire a che velocita' avverra' il raggiungimento di target_x_appo! piu' alta la variabile piu' lento il movimento 2988 equilibrio_per_ricarica(); 2989/* 2990 // -------------------------------------------------------- RADRIZZAMENTO ZAMPE A FINE RICARICA ZAMPA 3 2991 if(flag_fine_raddrizzamento2 == 0) 2992 { 2993 zampa2_no_equilibrio = 1; 2994 2995 if(pos_avambraccio2_fin != flag_memorizz_coord_avambra2 && pos_braccio2_fin != flag_memorizz_coord_bra2) 2996 { 2997 movimento_zampa_new2(1 , 2, flag_memorizz_coord_avambra2); //avambr 20 2998 movimento_zampa_new2(2 , 1, flag_memorizz_coord_bra2); //bra 30 2999 } 3000 else 3001 { 3002 flag_fine_raddrizzamento2 = 1; 3003 zampa2_no_equilibrio = 0; 3004 } 3005 } //fine if(flag_fine_raddrizzamento2 == 0) 3006*/ 3007 3008 3009 // ----------------------------------- RAGGIUNTA L'INCLINAZIONE TARGET 0 3010 if(angolo_roll_attuale == target_x && angolo_pitch_attuale == target_y) // && pos_spalla1_fin == 70 && pos_spalla3_fin == 70 && pos_spalla2_fin == 50 && pos_spalla4_fin == 50) 3011 { 3012 timer_generico2 = timer_generico2 + 1; 3013 if(timer_generico2 >= durata_timer) 3014 { 3015 3016 zampa3_no_equilibrio = 0; //EQUILIBRIO RIABILITATO PER ZAMPA 3 3017 3018 3019 timer_generico = 0; 3020 timer_generico2 = 0; 3021 timer_generico3 = 0; 3022 timer_generico_equilibrio = 0; 3023 timer_raggiung_target_x = 0; 3024 timer_raggiung_target_y = 0; 3025 flag_fine_raddrizzamento1 = 0, flag_fine_raddrizzamento2 = 0, flag_fine_raddrizzamento3 = 0, flag_fine_raddrizzamento4 = 0; 3026 fase_mov_zampe = 16; 3027 } 3028 } //fine if(target_x == angolo_roll_attuale && target_y == angolo_pitch_attuale) 3029 3030 }//fine if(ok_zampa1 == 1) 3031} //fine if (fase_mov_zampe == 15) 3032 3033 3034// FASE 16 ---------------------------------------------------------------------------------------------------- 3035//-------------------------------------------- RESET ALTEZZA ROBOT -------------------------------------------- 3036//------------------------------------------------------------------------------------------------------------- 3037if (fase_mov_zampe == 16) 3038{ 3039 3040 zampa1_no_equilibrio = 1; 3041 zampa2_no_equilibrio = 1; 3042 zampa3_no_equilibrio = 1; 3043 zampa4_no_equilibrio = 1; 3044 3045 3046 altezza_zampe(); //220 e' l'altezza iniziale 3047 3048 //SE ALTEZZA ROBOT INFERIORE ALLA MEDIA (211 / 229) ALLORA IL ROBOT SI ALZA 3049 if(altezza_robot_totale <= 230) 3050 { 3051 modo_altezza1 = 1; //zampa si allunga 3052 modo_altezza2 = 1; //zampa si allunga 3053 modo_altezza3 = 1; //zampa si allunga 3054 modo_altezza4 = 1; //zampa si allunga 3055 } 3056 //SE ALTEZZA ROBOT SUPERIORE ALLA MEDIA (211 / 229) ALLORA IL ROBOT SI ABBASSA 3057 if(altezza_robot_totale >= 240) 3058 { 3059 modo_altezza1 = 2; //zampa si abbassa 3060 modo_altezza2 = 2; //zampa si abbassa 3061 modo_altezza3 = 2; //zampa si abbassa 3062 modo_altezza4 = 2; //zampa si abbassa 3063 } 3064 //SE ALTEZZA ROBOT RIENTRA NELL'AREA MEDIA (211 / 229) ALLORA IL ROBOT HA ESEGUITO IL RESET ALTEZZA! 3065 if(altezza_robot_totale > 230 && altezza_robot_totale < 240) 3066 { 3067 3068 modo_altezza1 = 0; //zampa si abbassa 3069 modo_altezza2 = 0; //zampa si abbassa 3070 modo_altezza3 = 0; //zampa si abbassa 3071 modo_altezza4 = 0; //zampa si abbassa 3072 3073 zampa1_no_equilibrio = 0; 3074 zampa2_no_equilibrio = 0; 3075 zampa3_no_equilibrio = 0; 3076 zampa4_no_equilibrio = 0; 3077 fase_mov_zampe = 17; 3078 } 3079} 3080 3081 3082 3083// FASE 17 ---------------------------------------------------------------------------------------------------- 3084//-------------------------------------------- TUTTE LE ZAMPE SPINGONO! 4 -------------------------------------- 3085//------------------------------------------------------------------------------------------------------------ 3086if (fase_mov_zampe == 17) 3087{ 3088 //riflessi_massimi = 1; 3089 //velocita_spinta = 1; //piu alto = piu' lento 3090 3091 lentezza_spinte = 3; 3092 spinta_zampe = 1; 3093 3094 timer_generico = timer_generico + 1; 3095 if(timer_generico >= durata_spinte) 3096 { 3097 timer_generico2 = timer_generico2 + 1; 3098 if (timer_generico2 == durata_timer) 3099 { 3100 timer_generico = 0; 3101 timer_generico2 = 0; 3102 timer_generico3 = 0; 3103 spinta_zampe = 0; 3104 fase_mov_zampe = 0; 3105 } 3106 } 3107}// fine if (fase_mov_zampe == 17) 3108 3109 3110 3111 3112 3113 3114 3115 3116 // QUESTO E' L'ULTIMO CODICE GESTIONE SERVI! 8/1/2017 3117 //************************************************************************************** ZAMPA 1 SETUP 3118 //SETTAGGIO INIZIALE AVAMBRACCIO - 0 3119 new_pos_avambraccio1 = 180 - pos_avambraccio1_fin; 3120 pulselengthz = map(new_pos_avambraccio1, 0, 180, SERVOMIN_ava1, SERVOMAX_ava1); 3121 pwm.setPWM(0, 0, pulselengthz); 3122 3123 //SETTAGGIO INIZIALE BRACCIO - 1 3124 pulselengthzb = map(pos_braccio1_fin, 0, 180, SERVOMIN_bra1, SERVOMAX_bra1); 3125 pwm.setPWM(1, 0, pulselengthzb); 3126 3127 //SETTAGGIO INIZIALE SPALLA - 2 3128 new_pos_spalla1 = 180 - pos_spalla1_fin; 3129 pulselengthzc = map(new_pos_spalla1, 0, 180, SERVOMIN_spa1, SERVOMAX_spa1); 3130 pwm.setPWM(2, 0, pulselengthzc); 3131 3132 //************************************************************************************** ZAMPA 2 SETUP ---------- Con variabili fin! 3133 //SETTAGGIO INIZIALE AVAMBRACCIO - 4 3134 new_pos_avambraccio2 = 180 - pos_avambraccio2_fin; 3135 pulselengthz = map(new_pos_avambraccio2, 0, 180, SERVOMIN_ava2, SERVOMAX_ava2); 3136 pwm.setPWM(4, 0, pulselengthz); 3137 3138 //SETTAGGIO INIZIALE BRACCIO - 6 3139 new_pos_braccio2 = 180 - pos_braccio2_fin; 3140 pulselengthzb = map(new_pos_braccio2, 0, 180, SERVOMIN_bra2, SERVOMAX_bra2); 3141 pwm.setPWM(6, 0, pulselengthzb); 3142 3143 //SETTAGGIO INIZIALE SPALLA - 7 3144 new_pos_spalla2 = 180 - pos_spalla2_fin; 3145 pulselengthzc = map(new_pos_spalla2, 0, 180, SERVOMIN_spa2, SERVOMAX_spa2); 3146 pwm.setPWM(7, 0, pulselengthzc); 3147 3148 3149 //************************************************************************************** ZAMPA 3 SETUP 3150 //SETTAGGIO INIZIALE AVAMBRACCIO - 8 3151 pos_avambraccio3_fin = pos_avambraccio3_fin + 0;// e' mirror quindi + 10 3152 new_pos_avambraccio3 = 180 - pos_avambraccio3_fin; 3153 pulselengthz = map(new_pos_avambraccio3, 0, 180, SERVOMIN_ava3, SERVOMAX_ava3); 3154 pwm.setPWM(8, 0, pulselengthz); 3155 3156 //SETTAGGIO INIZIALE BRACCIO - 9 3157 pulselengthzb = map(pos_braccio3_fin, 0, 180, SERVOMIN_bra3, SERVOMAX_bra3); 3158 pwm.setPWM(9, 0, pulselengthzb); 3159 3160 //SETTAGGIO INIZIALE SPALLA - 10 3161 //new_pos_spalla3 = 180 - pos_spalla3_fin; 3162 //pulselengthzc = map(new_pos_spalla3, 0, 180, SERVOMIN_spa3, SERVOMAX_spa3); 3163 //pwm.setPWM(10, 0, pulselengthzc); 3164 3165 3166 //************************************************************************************** ZAMPA 4 SETUP 3167 //SETTAGGIO INIZIALE AVAMBRACCIO - 12 3168 new_pos_avambraccio4 = pos_avambraccio4_fin; 3169 pulselengthz = map(new_pos_avambraccio4, 0, 180, SERVOMIN_ava4, SERVOMAX_ava4); 3170 pwm.setPWM(12, 0, pulselengthz); 3171 3172 //SETTAGGIO INIZIALE BRACCIO - 13 3173 new_pos_braccio4 = 180 - pos_braccio4_fin; 3174 pulselengthzb = map(new_pos_braccio4, 0, 180, SERVOMIN_bra4, SERVOMAX_bra4); 3175 pwm.setPWM(13, 0, pulselengthzb); 3176 3177 //SETTAGGIO INIZIALE SPALLA - 15 3178 //new_pos_spalla4 = pos_spalla4_fin; 3179 //pulselengthzc = map(new_pos_spalla4, 0, 180, SERVOMIN_spa4, SERVOMAX_spa4); 3180 //pwm.setPWM(15, 0, pulselengthzc); 3181 3182 3183 3184 3185 3186} //fine else risposta_direzione = "GYRO OK"; 3187}//movimento_on = 1 3188}//fine if (start == 7) 3189 3190 //}//fine else if (mpuIntStatus & 0x02) { 3191 //wdt_reset(); WATCHDOG 3192 //delay(10); 3193 //mpu.resetFIFO(); 3194 }//fine LOOP 3195//______________________________________________________________________________________________________ 3196 3197 3198 3199 3200 3201void equilibrio_per_ricarica() 3202{ 3203 //fase di inclinazione X ---------> TARGET_X 3204 if(target_x != target_x_appo)//12 3205 { 3206 if(target_x < target_x_appo) 3207 { 3208 timer_raggiung_target_x = timer_raggiung_target_x + 1; 3209 if(timer_raggiung_target_x == tempo_attesa_appo) 3210 { 3211 target_x = target_x + 1; 3212 timer_raggiung_target_x = 0; 3213 } 3214 } 3215 if(target_x > target_x_appo) 3216 { 3217 timer_raggiung_target_x = timer_raggiung_target_x + 1; 3218 if(timer_raggiung_target_x == tempo_attesa_appo) 3219 { 3220 target_x = target_x - 1; 3221 timer_raggiung_target_x = 0; 3222 } 3223 } 3224 } //fine if(target_x != target_x_appo) 3225 3226 3227 3228 //fase di inclinazione Y ---------> TARGET_Y 3229 if(target_y != target_y_appo) 3230 { 3231 if(target_y < target_y_appo) 3232 { 3233 timer_raggiung_target_y = timer_raggiung_target_y + 1; 3234 if(timer_raggiung_target_y == tempo_attesa_appo) 3235 { 3236 target_y = target_y + 1; 3237 timer_raggiung_target_y = 0; 3238 } 3239 } 3240 if(target_y > target_y_appo) 3241 { 3242 timer_raggiung_target_y = timer_raggiung_target_y + 1; 3243 if(timer_raggiung_target_y == tempo_attesa_appo) 3244 { 3245 target_y = target_y - 1; 3246 timer_raggiung_target_y = 0; 3247 } 3248 } 3249 } //fine if(target_y != target_x_appo) 3250 3251} 3252 3253 3254 3255 3256//------------------------------------------------------------------------------------------------- 3257//------------------------------------------------------------------------------------------------- 3258//------------------------------------------------------------------------------------------------- 3259//------------------------------------------------------------------------------------------------- 3260//------------------------------------------------------------------------------------------------- 3261//------------------------------------------------------------------------------------------------- 3262//------------------------------------------------------------------------------------------------- 3263//------------------------------------------------------------------------------------------------- 3264 3265void ricarica_zampa1() //parte solo se c'e' ok_zampa2 = 1! 3266{ 3267 3268 //movimento_zampa_new1(int servo_num, int st_speed, int target) 3269 3270 3271 3272 // ********************************************************************************RICARICA ZAMPA 1 3273 // ------------------------------------ AVAMBRACCIO 1 SI CHIUDE 3274 if (moto_zampa1 == 1) 3275 { 3276 movimento_zampa_new1(1,8,40);//40 sollevamento_avambr_ant_ricarica vel8 3277 3278 if (pos_avambraccio1_fin == 40) 3279 { 3280 timer_ricarica1 = 0; 3281 moto_zampa1 = 2; 3282 } 3283 } 3284 3285 // ------------------------------------- BRACCIO 1 AVANZA 3286 if (moto_zampa1 == 2) 3287 { 3288 movimento_zampa_new1(2,8,65); //80 bra<ccio vel 8 3289 3290 //l' avambraccio si apre quando il braccio supera apertura 60 3291 if (pos_braccio1_fin <= 65) 3292 { 3293 movimento_zampa_new1(1,4,140); // pos avambraccio continua fino a 140! bisogna fermarlo! 3294 } 3295 3296 //CHECK MULTIPLO SENSORE TATTILE 3297 if (fsrReading1 >= 300 && pos_braccio1_fin <= 65) //300 3298 { 3299 fsrReading1_check = fsrReading1_check + 1; 3300 } 3301 3302 3303 //FINE RICARICA ZAMPA 1 3304 if ((pos_braccio1_fin == 65 && pos_avambraccio1_fin == 140) || (fsrReading1_check == 2)) //era 3 3305 { 3306 //movimento_zampa_C(0,1,1,1,2,pos_avambraccio1_fin); 3307 3308 timer_ricarica1 = 0; 3309 ok_zampa1 = 1; //finita ricarica 3310 moto_zampa1 = 3; 3311 fsrReading1_check = 0; 3312 } 3313 } 3314} 3315 3316 3317 3318void ricarica_zampa2() //parte solo se c'e' ok_zampa2 = 1! 3319{ 3320 3321 //void movimento_zampa_B(int flag_movimento_zampa, int zampa_num, int servo_num, int attesa_servo , int st_speed, int target) 3322 3323 // ********************************************************************************RICARICA ZAMPA 2 3324 // ------------------------------------ BRACCIO SI ALZA e AVAMBRACCIO 2 SI CHIUDE 3325 if (moto_zampa2 == 1) 3326 { 3327 //pausa_spinta_zampe = 1;//STOP TIMER CAMMINATA! 3328 3329 //movimento_zampa_fast(0,2,1,40);//30 sollevamento_avambr_ant_ricarica 3330 //movimento_zampa(0,2,1,1,40);//40 sollevamento_avambr_ant_ricarica 3331 3332 movimento_zampa_new2(2,8,130);// 3333 movimento_zampa_new2(1,8,20);//40 sollevamento_avambr_ant_ricarica vel8 3334 3335 if (pos_avambraccio2_fin == 20 && pos_braccio2_fin == 130) 3336 { 3337 //timer_ricarica2 = timer_ricarica2 + 1; 3338 //if (timer_ricarica2 > 10) //10 3339 //{ 3340 timer_ricarica2 = 0; 3341 moto_zampa2 = 2; 3342 //} 3343 } 3344 } 3345 3346 //ricarica zampa 3 e 4 devono spingere con gli avambracci! (aprendoli!) 3347 3348 // ------------------------------------- BRACCIO 2 AVANZA 3349 if (moto_zampa2 == 2) 3350 { 3351 //movimento_zampa_fast(0,2,2,65); //parte da 110 a 95 3352 //movimento_zampa(0,2,2,1,20); //50 3353 movimento_zampa_new2(2,8,65); //80 bra<ccio vel 8 3354 3355 //l' avambraccio si apre quando il braccio supera apertura 60 3356 if (pos_braccio2_fin <= 65) 3357 { 3358 movimento_zampa_new2(1,4,140); // 85 pos avambraccio continua fino a 140! bisogna fermarlo! 3359 } 3360 3361 3362 //CHECK MULTIPLO SENSORE TATTILE 3363 if (fsrReading2 >= 300) 3364 { 3365 fsrReading2_check = fsrReading2_check + 1; 3366 } 3367 3368 //FINE RICARICA ZAMPA 2 3369 if ((pos_braccio2_fin == 65 && pos_avambraccio2_fin == 140) || (fsrReading2_check == 2)) //era 3 3370 { 3371 //movimento_zampa_C(0,1,1,1,2,pos_avambraccio1_fin); 3372 3373 timer_ricarica2 = 0; 3374 ok_zampa2 = 1; //finita ricarica 3375 moto_zampa2 = 3; 3376 fsrReading2_check = 0; 3377 } 3378 } 3379} 3380 3381 3382 3383 3384void ricarica_zampa3() //parte solo se c'e' ok_zampa3 = 1! 3385{ 3386 // ********************************************************************************RICARICA ZAMPA 3 3387 // --------------------------- AVAMBRACCIO 3 SI CHIUDE + BRACCIO 3 SI ALZA (AVANZA) 3388 if (moto_zampa3 == 1) 3389 { 3390 3391 movimento_zampa_new3(1,8,20);//20 avambraccio vel8 3392 movimento_zampa_new3(2,4,10);// braccio vel8 3393 3394 if (pos_avambraccio3_fin == 20 && pos_braccio3_fin == 10) 3395 { 3396 timer_ricarica3 = 0; 3397 moto_zampa3 = 2; 3398 } 3399 } //fine if (moto_zampa3 == 1) 3400 3401 3402 // ------------------------------------- AVAMBRACCIO 3 SI APRE E BRACCIO SCENDE!! 3403 if (moto_zampa3 == 2) 3404 { 3405 //timer_ricarica3 = timer_ricarica3 + 1; 3406 3407 movimento_zampa_new3(1,8,130); //avambraccio 3 si apre fino a 55 vel8 3408 3409 //il braccio si abbassa quando l'avambraccio supera apertura 35 3410 if (pos_avambraccio3_fin >= 50) 3411 { 3412 //140=MAX(braccio tutto dietro 3413 movimento_zampa_new3(2,4,55); //braccio 40 3414 } 3415 3416 //CHECK MULTIPLO SENSORE TATTILE 3417 if (fsrReading3 >= 300) 3418 { 3419 fsrReading3_check = fsrReading3_check + 1; 3420 } 3421 3422 //FINE RICARICA ZAMPA 3 3423 if ((pos_avambraccio3_fin == 130 && pos_braccio3_fin == 55) || (fsrReading3_check == 2)) //era 3 3424 { 3425 //movimento_zampa_C(0,3,1,1,2,pos_avambraccio4_fin); 3426 timer_ricarica3 = 0; 3427 ok_zampa3 = 1; //finita ricarica 3428 moto_zampa3 = 3; 3429 fsrReading3_check = 0; 3430 } 3431 } //fine if (moto_zampa4 == 2) 3432 3433} //fine void ricarica_zampa4() 3434 3435 3436 3437 3438 3439 3440void ricarica_zampa4() //parte solo se c'e' ok_zampa4 = 1! 3441{ 3442 // ********************************************************************************RICARICA ZAMPA 4 3443 // --------------------------- AVAMBRACCIO 4 SI CHIUDE + BRACCIO 4 SI ALZA (AVANZA) 3444 if (moto_zampa4 == 1) 3445 { 3446 3447 movimento_zampa_new4(1,8,20);//20 avambraccio vel4 3448 movimento_zampa_new4(2,4,10);// braccio vel2 3449 3450 if (pos_avambraccio4_fin == 20 && pos_braccio4_fin == 10) 3451 { 3452 timer_ricarica4 = 0; 3453 moto_zampa4 = 2; 3454 } 3455 } //fine if (moto_zampa4 == 1) 3456 3457 3458 3459 // ------------------------------------- AVAMBRACCIO 4 SI APRE E BRACCIO SCENDE!! 3460 if (moto_zampa4 == 2) 3461 { 3462 timer_ricarica4 = timer_ricarica4 + 1; 3463 3464 3465 movimento_zampa_new4(1,8,130); //avambraccio 4 si apre fino a 70 vel2 3466 3467 //il braccio si abbassa quando l'avambraccio supera apertura tot 3468 if (pos_avambraccio4_fin >= 50) //45 3469 { 3470 3471 // 140=MAX(braccio tutto dietro 3472 movimento_zampa_new4(2,4,55); //braccio 45 3473 } 3474 3475 //CHECK MULTIPLO SENSORE TATTILE 3476 if (fsrReading4 >= 300) 3477 { 3478 fsrReading4_check = fsrReading4_check + 1; 3479 } 3480 3481 //FINE RICARICA ZAMPA 4 3482 if ((pos_avambraccio4_fin == 130 && pos_braccio4_fin == 55) || (fsrReading4_check == 2)) // && timer_ricarica4 > 5)//timer_ricarica = 5 3483 { 3484 //movimento_zampa_C(0,4,1,1,2,pos_avambraccio4_fin); 3485 timer_ricarica4 = 0; 3486 ok_zampa4 = 1; //finita ricarica 3487 moto_zampa4 = 3; 3488 fsrReading4_check = 0; 3489 } 3490 } //fine if (moto_zampa4 == 2) 3491 3492} //fine void ricarica_zampa4() 3493 3494 3495 3496 3497void movimento_zampa_new1(int servo_num, int st_speed, int target) 3498{ 3499 3500 ///////////////////////////////////////////////////////////////////////////////////////////// 3501 /////////////////////////////////////////////////////////////////////////// ZAMPA 1 3502 ///////////////////////////////////////////////////////////////////////////////////////////// 3503 if (servo_num == 1) //AVAMBRACCIO 3504 { 3505 //timer_ava2 = timer_ava2 + 1; 3506 //----------------------------------------------------------------SE POSIZIONE ATTUALE < target 3507 if (pos_avambraccio1_fin < target) 3508 { 3509 //timer_ava2 = 0; 3510 pos_avambraccio1_fin = pos_avambraccio1_fin + st_speed; 3511 3512 //CHECK POSIZIONE 3513 if(pos_avambraccio1_fin >= target) 3514 { 3515 pos_avambraccio1_fin = target; 3516 } 3517 } 3518 //----------------------------------------------------------------SE POSIZIONE ATTUALE > target 3519 if (pos_avambraccio1_fin > target) 3520 { 3521 //timer_ava2 = 0; 3522 pos_avambraccio1_fin = pos_avambraccio1_fin - st_speed; 3523 3524 //CHECK POSIZIONE 3525 if(pos_avambraccio1_fin <= target) 3526 { 3527 pos_avambraccio1_fin = target; 3528 } 3529 } 3530 //----------------------------------------------------------------SE POSIZIONE ROOT == target 3531 if (pos_avambraccio1_fin == target) 3532 { 3533 //timer_ava2 = 0; 3534 pos_avambraccio1_fin = target; 3535 } 3536 } 3537 3538 if (servo_num == 2) //BRACCIO 3539 { 3540 //timer_ava2 = timer_ava2 + 1; 3541 3542 //----------------------------------------------------------------SE POSIZIONE ATTUALE < target 3543 if (pos_braccio1_fin < target) 3544 { 3545 //timer_ava2 = 0; 3546 pos_braccio1_fin = pos_braccio1_fin + st_speed; 3547 3548 //CHECK POSIZIONE 3549 if(pos_braccio1_fin >= target) 3550 { 3551 pos_braccio1_fin = target; 3552 } 3553 } 3554 //----------------------------------------------------------------SE POSIZIONE ATTUALE > target 3555 if (pos_braccio1_fin > target) 3556 { 3557 //timer_ava2 = 0; 3558 pos_braccio1_fin = pos_braccio1_fin - st_speed; 3559 3560 //CHECK POSIZIONE 3561 if(pos_braccio1_fin <= target) 3562 { 3563 pos_braccio1_fin = target; 3564 } 3565 } 3566 //----------------------------------------------------------------SE POSIZIONE ROOT == target 3567 if (pos_braccio1_fin == target) 3568 { 3569 //timer_ava2 = 0; 3570 pos_braccio1_fin = target; 3571 } 3572 } 3573} 3574 3575 3576void movimento_zampa_new2(int servo_num, int st_speed, int target) 3577{ 3578 ///////////////////////////////////////////////////////////////////////////////////////////// 3579 /////////////////////////////////////////////////////////////////////////// ZAMPA 2 3580 ///////////////////////////////////////////////////////////////////////////////////////////// 3581 if (servo_num == 1) //AVAMBRACCIO 3582 { 3583 //timer_ava2 = timer_ava2 + 1; 3584 //----------------------------------------------------------------SE POSIZIONE ATTUALE < target 3585 if (pos_avambraccio2_fin < target) 3586 { 3587 //timer_ava2 = 0; 3588 pos_avambraccio2_fin = pos_avambraccio2_fin + st_speed; 3589 3590 //CHECK POSIZIONE 3591 if(pos_avambraccio2_fin >= target) 3592 { 3593 pos_avambraccio2_fin = target; 3594 } 3595 } 3596 //----------------------------------------------------------------SE POSIZIONE ATTUALE > target 3597 if (pos_avambraccio2_fin > target) 3598 { 3599 //timer_ava2 = 0; 3600 pos_avambraccio2_fin = pos_avambraccio2_fin - st_speed; 3601 3602 //CHECK POSIZIONE 3603 if(pos_avambraccio2_fin <= target) 3604 { 3605 pos_avambraccio2_fin = target; 3606 } 3607 } 3608 //----------------------------------------------------------------SE POSIZIONE ROOT == target 3609 if (pos_avambraccio2_fin == target) 3610 { 3611 //timer_ava2 = 0; 3612 pos_avambraccio2_fin = target; 3613 } 3614 } 3615 3616 if (servo_num == 2) //BRACCIO 3617 { 3618 //timer_ava2 = timer_ava2 + 1; 3619 3620 //----------------------------------------------------------------SE POSIZIONE ATTUALE < target 3621 if (pos_braccio2_fin < target) 3622 { 3623 //timer_ava2 = 0; 3624 pos_braccio2_fin = pos_braccio2_fin + st_speed; 3625 3626 //CHECK POSIZIONE 3627 if(pos_braccio2_fin >= target) 3628 { 3629 pos_braccio2_fin = target; 3630 } 3631 } 3632 //----------------------------------------------------------------SE POSIZIONE ATTUALE > target 3633 if (pos_braccio2_fin > target) 3634 { 3635 //timer_ava2 = 0; 3636 pos_braccio2_fin = pos_braccio2_fin - st_speed; 3637 3638 //CHECK POSIZIONE 3639 if(pos_braccio2_fin <= target) 3640 { 3641 pos_braccio2_fin = target; 3642 } 3643 } 3644 //----------------------------------------------------------------SE POSIZIONE ROOT == target 3645 if (pos_braccio2_fin == target) 3646 { 3647 //timer_ava2 = 0; 3648 pos_braccio2_fin = target; 3649 } 3650 } 3651} 3652 3653 3654 3655void movimento_zampa_new3(int servo_num, int st_speed, int target) 3656{ 3657 ///////////////////////////////////////////////////////////////////////////////////////////// 3658 /////////////////////////////////////////////////////////////////////////// ZAMPA 3 3659 ///////////////////////////////////////////////////////////////////////////////////////////// 3660 if (servo_num == 1) //AVAMBRACCIO 3661 { 3662 //timer_ava2 = timer_ava2 + 1; 3663 //----------------------------------------------------------------SE POSIZIONE ATTUALE < target 3664 if (pos_avambraccio3_fin < target) 3665 { 3666 //timer_ava2 = 0; 3667 pos_avambraccio3_fin = pos_avambraccio3_fin + st_speed; 3668 3669 //CHECK POSIZIONE 3670 if(pos_avambraccio3_fin >= target) 3671 { 3672 pos_avambraccio3_fin = target; 3673 } 3674 } 3675 //----------------------------------------------------------------SE POSIZIONE ATTUALE > target 3676 if (pos_avambraccio3_fin > target) 3677 { 3678 //timer_ava2 = 0; 3679 pos_avambraccio3_fin = pos_avambraccio3_fin - st_speed; 3680 3681 //CHECK POSIZIONE 3682 if(pos_avambraccio3_fin <= target) 3683 { 3684 pos_avambraccio3_fin = target; 3685 } 3686 } 3687 //----------------------------------------------------------------SE POSIZIONE ROOT == target 3688 if (pos_avambraccio3_fin == target) 3689 { 3690 //timer_ava2 = 0; 3691 pos_avambraccio3_fin = target; 3692 } 3693 } 3694 3695 if (servo_num == 2) //BRACCIO 3696 { 3697 //timer_ava2 = timer_ava2 + 1; 3698 3699 //----------------------------------------------------------------SE POSIZIONE ATTUALE < target 3700 if (pos_braccio3_fin < target) 3701 { 3702 //timer_ava2 = 0; 3703 pos_braccio3_fin = pos_braccio3_fin + st_speed; 3704 3705 //CHECK POSIZIONE 3706 if(pos_braccio3_fin >= target) 3707 { 3708 pos_braccio3_fin = target; 3709 } 3710 } 3711 //----------------------------------------------------------------SE POSIZIONE ATTUALE > target 3712 if (pos_braccio3_fin > target) 3713 { 3714 //timer_ava2 = 0; 3715 pos_braccio3_fin = pos_braccio3_fin - st_speed; 3716 3717 //CHECK POSIZIONE 3718 if(pos_braccio3_fin <= target) 3719 { 3720 pos_braccio3_fin = target; 3721 } 3722 } 3723 //----------------------------------------------------------------SE POSIZIONE ROOT == target 3724 if (pos_braccio3_fin == target) 3725 { 3726 //timer_ava2 = 0; 3727 pos_braccio3_fin = target; 3728 } 3729 } 3730} 3731 3732void movimento_zampa_new4(int servo_num, int st_speed, int target) 3733{ 3734 ///////////////////////////////////////////////////////////////////////////////////////////// 3735 /////////////////////////////////////////////////////////////////////////// ZAMPA 4 3736 ///////////////////////////////////////////////////////////////////////////////////////////// 3737 if (servo_num == 1) //AVAMBRACCIO 3738 { 3739 //timer_ava2 = timer_ava2 + 1; 3740 3741 //----------------------------------------------------------------SE POSIZIONE ATTUALE < target 3742 if (pos_avambraccio4_fin < target) 3743 { 3744 //timer_ava2 = 0; 3745 pos_avambraccio4_fin = pos_avambraccio4_fin + st_speed; 3746 3747 //CHECK POSIZIONE 3748 if(pos_avambraccio4_fin >= target) 3749 { 3750 pos_avambraccio4_fin = target; 3751 } 3752 } 3753 //----------------------------------------------------------------SE POSIZIONE ATTUALE > target 3754 if (pos_avambraccio4_fin > target) 3755 { 3756 //timer_ava2 = 0; 3757 pos_avambraccio4_fin = pos_avambraccio4_fin - st_speed; 3758 3759 //CHECK POSIZIONE 3760 if(pos_avambraccio4_fin <= target) 3761 { 3762 pos_avambraccio4_fin = target; 3763 } 3764 } 3765 //----------------------------------------------------------------SE POSIZIONE ROOT == target 3766 if (pos_avambraccio4_fin == target) 3767 { 3768 //timer_ava2 = 0; 3769 pos_avambraccio4_fin = target; 3770 } 3771 } 3772 3773 if (servo_num == 2) //BRACCIO 3774 { 3775 //timer_ava2 = timer_ava2 + 1; 3776 //----------------------------------------------------------------SE POSIZIONE ATTUALE < target 3777 if (pos_braccio4_fin < target) 3778 { 3779 //timer_ava2 = 0; 3780 pos_braccio4_fin = pos_braccio4_fin + st_speed; 3781 3782 //CHECK POSIZIONE 3783 if(pos_braccio4_fin >= target) 3784 { 3785 pos_braccio4_fin = target; 3786 } 3787 } 3788 //----------------------------------------------------------------SE POSIZIONE ATTUALE > target 3789 if (pos_braccio4_fin > target) 3790 { 3791 //timer_ava2 = 0; 3792 pos_braccio4_fin = pos_braccio4_fin - st_speed; 3793 3794 //CHECK POSIZIONE 3795 if(pos_braccio4_fin <= target) 3796 { 3797 pos_braccio4_fin = target; 3798 } 3799 } 3800 //----------------------------------------------------------------SE POSIZIONE ROOT == target 3801 if (pos_braccio4_fin == target) 3802 { 3803 //timer_ava2 = 0; 3804 pos_braccio4_fin = target; 3805 } 3806 } 3807} 3808 3809 3810 3811 3812 3813 3814 3815 3816 3817void equilibrio_spalle() 3818{ 3819 3820 //---------------------------------------------------------------------- 3821 //----------------------------------------------------- ASSE X 3822 //---------------------------------------------------------------------- 3823 //ROBOT DRITTO 3824 if (angolo_roll_attuale == 0) 3825 { 3826 //fermi 2 e 4 3827 valore_tar_sin2 = 0; 3828 valore_tar_sin4 = 0; 3829 //fermi 1 e 3 3830 valore_tar_des1 = 0; 3831 valore_tar_des3 = 0; 3832 } 3833 3834 //INCLINAZIONE A SINISTRA 3835 if (angolo_roll_attuale > 0) 3836 { 3837 //Apertura 2 e 4 3838 valore_tar_sin2 = 1; 3839 valore_tar_sin4 = 1; 3840 //Chiusura 1 e 3 3841 valore_tar_des1 = -1; 3842 valore_tar_des3 = -1; 3843 //fase_equilibrio = 1; 3844 } 3845 3846 //INCLINAZIONE A DESTRA 3847 if (angolo_roll_attuale < 0) 3848 { 3849 //Apertura 2 e 4 3850 valore_tar_sin2 = -1; 3851 valore_tar_sin4 = -1; 3852 //Chiusura 1 e 3 3853 valore_tar_des1 = 1; 3854 valore_tar_des3 = 1; 3855 //fase_equilibrio = 1; 3856 } 3857 3858 3859 //movimento_zampa_C(0, 1, 3, 1 , 1, pos_spalla1_fin+valore_tar_sin2) ; 3860 //movimento_zampa_C(0, 2, 3, 1 , 1, pos_spalla2_fin+valore_tar_sin2) ; 3861 3862 3863 3864/* 3865//SPALLA 2 3866int pos_spalla2 = 60; // 120 =MAX (spalla tutta chiusa) 3867 // 0 =MIN (spalla tutta aperta) 3868 // 60 = MED 3869*/ 3870 3871 3872 //--- 3873 3874 if (valore_tar_sin2 == 0) 3875 { 3876 movimento_zampa_new1(3, 1, 60); //zampa 1 3877 movimento_zampa_new3(3, 1, 60); //zampa 3 3878 3879 movimento_zampa_new2(3, 1, 60); //zampa 2 3880 movimento_zampa_new4(3, 1, 60); //zampa 4 3881 } 3882 3883 if (valore_tar_sin2 == 1) 3884 { 3885 movimento_zampa_new1(3 , 1, 50); //zampa 1 3886 movimento_zampa_new3(3 , 1, 50); //zampa 3 3887 3888 movimento_zampa_new2(3 , 1, 70); //zampa 2 3889 movimento_zampa_new4(3 , 1, 70); //zampa 4 3890 } 3891 3892 if (valore_tar_sin2 == -1) 3893 { 3894 movimento_zampa_new1(3, 1, 70); //zampa 1 3895 movimento_zampa_new3(3, 1, 70); //zampa 3 3896 3897 movimento_zampa_new2(3, 1, 50); //zampa 2 3898 movimento_zampa_new4(3, 1, 50); //zampa 4 3899 } 3900 //--- 3901} 3902 3903 3904 3905 3906 3907 3908 3909void spinta_zampe_codice() 3910{ 3911 crono_spinte = crono_spinte + 1; 3912 if (crono_spinte == lentezza_spinte) 3913 { 3914 //------------------------------ ZAMPA 1 3915 //SPINTA BRACCI 1,2,3,4 3916 //if(pos_braccio1_fin <= 130){ 3917 movimento_zampa_new1(2,3,140); //SPINTA BRACCIO 1! 110 3918 if (pos_braccio1_fin >= 110) //100 3919 { 3920 movimento_zampa_new1(1,2,140); //SPINTA AVAMBRACCIO 1! 3921 } 3922 //} 3923 3924 //------------------------------ ZAMPA 2 3925 //if(pos_braccio2_fin <= 130){ 3926 movimento_zampa_new2(2,3,140); //SPINTA BRACCIO 2! 110 3927 if (pos_braccio2_fin >= 110) //100 3928 { 3929 movimento_zampa_new2(1,2,140); //SPINTA AVAMBRACCIO 2! 3930 } 3931 //} 3932 3933 3934 //------------------------------ ZAMPA 3 3935 //if(pos_braccio3_fin <= 100){ 3936 movimento_zampa_new3(2,2,100); //SPINTA BRACCIO 3! 100 3937 //se il braccio e' <= di 90, il polpaccio si chiude lentamente 3938 3939 //se il braccio e' > di 90, il polpaccio si apre velocemente 3940 if (pos_braccio3_fin > 30) //90 3941 { 3942 movimento_zampa_new3(1,2,140); 3943 } 3944 3945 3946 //------------------------------ ZAMPA 4 3947 //if(pos_braccio4_fin <= 100){ 3948 movimento_zampa_new4(2,2,100); //SPINTA BRACCIO 4! 100 3949 3950 //se il braccio e' > di 90, il polpaccio si apre lentamente 3951 if (pos_braccio4_fin > 30) //90 3952 { 3953 movimento_zampa_new4(1,2,140); 3954 } 3955 3956 3957 crono_spinte = 0; 3958 }//fine if (crono_spinte == lentezza_spinte) 3959} 3960 3961 3962/* 3963void spinta_zampe_codice() 3964{ 3965 crono_spinte = crono_spinte + 1; 3966 if (crono_spinte == lentezza_spinte) 3967 { 3968 3969 //------------------------------ ZAMPA 1 3970 //SPINTA BRACCI 1,2,3,4 3971 movimento_zampa_new1(2,3,140); //SPINTA BRACCIO 1! 110 3972 if (pos_braccio1_fin >= 90) //100 3973 { 3974 3975 //se il peso e' troppo nel piede 3976 if (fsrReading1 > 400) 3977 { 3978 movimento_zampa_new1(1,2,20); //avambr chiude 3979 } 3980 //se il peso e' troppo POCO nel piede 3981 if (fsrReading1 < 400) 3982 { 3983 movimento_zampa_new1(1,2,140); //avambr 3984 } 3985 3986 } 3987 3988 3989 3990 //------------------------------ ZAMPA 2 3991 movimento_zampa_new2(2,3,140); //SPINTA BRACCIO 2! 110 3992 if (pos_braccio2_fin >= 90) //100 3993 { 3994 3995 //se il peso e' troppo nel piede 3996 if (fsrReading2 > 400) 3997 { 3998 movimento_zampa_new2(1,2,20); //avambr chiude 3999 } 4000 //se il peso e' troppo POCO nel piede 4001 if (fsrReading2 < 400) 4002 { 4003 movimento_zampa_new2(1,2,140); //avambr 4004 } 4005 4006 4007 } 4008 4009 4010 4011 //------------------------------ ZAMPA 3 4012 movimento_zampa_new3(2,2,100); //SPINTA BRACCIO 3! 100 4013 if (pos_braccio3_fin > 30) //90 4014 { 4015 4016 //se il peso e' troppo nel piede 4017 if (fsrReading3 > 400) 4018 { 4019 movimento_zampa_new3(1,2,50); 4020 } 4021 //se il peso e' troppo POCO nel piede 4022 if (fsrReading3 < 400) 4023 { 4024 movimento_zampa_new3(1,2,140); 4025 } 4026 4027 } 4028 4029 4030 4031 //------------------------------ ZAMPA 4 4032 movimento_zampa_new4(2,2,100); //SPINTA BRACCIO 4! 100 4033 if (pos_braccio4_fin > 30) //90 4034 { 4035 //se il peso e' troppo nel piede 4036 if (fsrReading4 > 400) 4037 { 4038 movimento_zampa_new4(1,2,50); 4039 } 4040 //se il peso e' troppo POCO nel piede 4041 if (fsrReading4 < 400) 4042 { 4043 movimento_zampa_new4(1,2,140); 4044 } 4045 } 4046 4047 4048 4049 4050 crono_spinte = 0; 4051 }//fine if (crono_spinte == lentezza_spinte) 4052} 4053*/ 4054 4055 4056 4057void altezza_zampe() 4058{ 4059 4060 altezza_robot_zampa1 = pos_avambraccio1_fin; 4061 altezza_robot_zampa2 = pos_avambraccio2_fin; 4062 altezza_robot_zampa3 = pos_avambraccio3_fin; 4063 altezza_robot_zampa4 = pos_avambraccio4_fin; 4064 altezza_robot_totale = altezza_robot_zampa1 + altezza_robot_zampa2 + altezza_robot_zampa3 + altezza_robot_zampa4; 4065 4066 4067 /////////////////////////////////////////////////////////////////////////////////////////// 4068 /////////////////////////////////////////////////////////////////////////////////////////// 4069 /////////////////////////////////////// ROUTINE ALTEZZA ROBOT ///////////////////////////// 4070 /////////////////////////////////////////////////////////////////////////////////////////// 4071 /////////////////////////////////////////////////////////////////////////////////////////// 4072//flag_altezza_serial == 1 4073//modo_altezza4 = 1 (aumenta) 2 (diminuisce) 4074 4075// ------------------------------------------ESECUZIONE ALTEZZA 4076//if (flag_altezza_serial == 1) 4077//{ 4078 /* 4079 // --------------------------------SETUP 4080 // pos_avambraccio1_alt_targ e' settato con pulsante piu' e meno 4081 //flag_altezza_serial = 1 quando do l'input da pulsante 4082 if (flag_altezza == 0) 4083 { 4084 pos_avambraccio2_alt_targ = pos_avambraccio1_alt_targ; 4085 pos_avambraccio3_alt_targ = pos_avambraccio1_alt_targ -10; 4086 pos_avambraccio4_alt_targ = pos_avambraccio1_alt_targ -10; 4087 flag_altezza = 1; //fine setup 4088 } 4089 */ 4090 4091/* 4092 //---ZAMPA 1 4093 if (pos_avambraccio1_fin < pos_avambraccio1_alt_targ) 4094 { 4095 modo_altezza1 = 1; //AUMENTA ALTEZZA ZAMPA 4096 } 4097 if (pos_avambraccio1_fin > pos_avambraccio1_alt_targ) 4098 { 4099 modo_altezza1 = 2; //DIMINUISCE ALTEZZA ZAMPA 4100 } 4101*/ 4102 4103/* 4104 //---ZAMPA 2 4105 if (pos_avambraccio2_fin < pos_avambraccio2_alt_targ) 4106 { 4107 modo_altezza2 = 1; //AUMENTA ALTEZZA ZAMPA 4108 } 4109 if (pos_avambraccio2_fin > pos_avambraccio2_alt_targ) 4110 { 4111 modo_altezza2 = 2; //DIMINUISCE ALTEZZA ZAMPA 4112 } 4113*/ 4114/* 4115 //---ZAMPA 3 4116 if (pos_avambraccio3_fin < pos_avambraccio3_alt_targ) 4117 { 4118 modo_altezza3 = 1; //AUMENTA ALTEZZA ZAMPA 4119 } 4120 if (pos_avambraccio3_fin > pos_avambraccio3_alt_targ) 4121 { 4122 modo_altezza3 = 2; //DIMINUISCE ALTEZZA ZAMPA 4123 } 4124 //---ZAMPA 4 4125 if (pos_avambraccio4_fin < pos_avambraccio4_alt_targ) 4126 { 4127 modo_altezza4 = 1; //AUMENTA ALTEZZA ZAMPA 4128 } 4129 if (pos_avambraccio4_fin > pos_avambraccio4_alt_targ) 4130 { 4131 modo_altezza4 = 2; //DIMINUISCE ALTEZZA ZAMPA 4132 } 4133*/ 4134 4135/* 4136 pausa_reflex = 1; 4137 speed_reflex = 4; //per il braccio sara' 1 mentre per l'avambr sara' speed_reflex * 2 4138*/ 4139 4140//void movimento_zampa_C1(int servo_num, int st_speed, int target) 4141 //----------------------------------------------- ZAMPA 1 ------------------------------------------------ 4142 4143 4144timerk = timerk + 1; 4145if(timerk == 2) 4146{ 4147 4148 if(modo_altezza1 == 0) 4149 { 4150 4151 movimento_zampa_new1(2,1,pos_braccio1_fin); //zampa1 vel 1 brac 4152 movimento_zampa_new1(1,2,pos_avambraccio1_fin); //zampa1 vel 2 avambr 4153 } 4154 4155 if(modo_altezza1 == 1) 4156 { 4157 //AUMENTA ALTEZZA ZAMPA, il braccio avanza e L'AVAMBRACCIO SI APRE! E FUNZIONA 4158 //quindi se la pos_avambraccio1_fin < del targer 4159 movimento_zampa_new1(2,1,70); //zampa1 vel 1 brac 4160 movimento_zampa_new1(1,2,120); //zampa1 vel 2 avambr 4161 } //fine if(modo_altezza == 1) 4162 4163 if(modo_altezza1 == 2) 4164 { 4165 //DIMINUISCE ALTEZZA ZAMPA, il braccio indietreggia e L'AVAMBRACCIO SI CHIUDE! E FUNZIONA 4166 //quindi se la pos_avambraccio2_fin < del targer 4167 movimento_zampa_new1(2,1,130); //zampa1 vel 1 brac 4168 movimento_zampa_new1(1,2,20); //zampa1 vel 2 avambr 4169 } //fine if(modo_altezza == 2) 4170 4171 4172 4173 4174 //----------------------------------------------- ZAMPA 2 ------------------------------------------------ 4175 if(modo_altezza2 == 0) 4176 { 4177 movimento_zampa_new2(2,1,pos_braccio2_fin); //zampa2 vel 1 brac 4178 movimento_zampa_new2(1,2,pos_avambraccio2_fin); //zampa2 vel 2 avambr 4179 } 4180 4181 if(modo_altezza2 == 1) 4182 { 4183 //AUMENTA ALTEZZA ZAMPA, il braccio avanza e L'AVAMBRACCIO SI APRE! E FUNZIONA 4184 //quindi se la pos_avambraccio2_fin < del targer 4185 movimento_zampa_new2(2,1,70); //zampa2 vel 1 brac 4186 movimento_zampa_new2(1,2,120); //zampa2 vel 2 avambr 4187 } //fine if(modo_altezza == 1) 4188 4189 if(modo_altezza2 == 2) 4190 { 4191 //DIMINUISCE ALTEZZA ZAMPA, il braccio indietreggia e L'AVAMBRACCIO SI CHIUDE! E FUNZIONA 4192 //quindi se la pos_avambraccio2_fin < del targer 4193 movimento_zampa_new2(2,1,130); //zampa2 vel 1 brac 4194 movimento_zampa_new2(1,2,20); //zampa2 vel 2 avambr 4195 } //fine if(modo_altezza == 2) 4196 4197 4198 //----------------------------------------------- ZAMPA 3 ------------------------------------------------ 4199 if(modo_altezza3 == 0) 4200 { 4201 movimento_zampa_new3(2,1,pos_braccio3_fin); //zampa3 vel 1 brac 4202 movimento_zampa_new3(1,2,pos_avambraccio3_fin); //zampa3 vel 2 avambr 4203 } 4204 4205 if(modo_altezza3 == 1)//pos < di targ 4206 { 4207 //AUMENTA ALTEZZA ZAMPA, il braccio avanza e L'AVAMBRACCIO SI APRE! E FUNZIONA 4208 //quindi se la pos_avambraccio2_fin < del targer 4209 movimento_zampa_new3(2,1,75); //zampa3 vel 1 brac 4210 movimento_zampa_new3(1,2,110); //zampa3 vel 2 avambr 4211 } //fine if(modo_altezza == 1) 4212 4213 if(modo_altezza3 == 2) 4214 { 4215 //DIMINUISCE ALTEZZA ZAMPA, il braccio indietreggia e L'AVAMBRACCIO SI CHIUDE! E FUNZIONA 4216 //quindi se la pos_avambraccio2_fin < del targer 4217 movimento_zampa_new3(2,1,20); //zampa3 vel 1 brac 4218 movimento_zampa_new3(1,2,10); //zampa3 vel 2 avambr 4219 } //fine if(modo_altezza == 2) 4220 4221 4222 //----------------------------------------------- ZAMPA 4 ------------------------------------------------ 4223 if(modo_altezza4 == 0) 4224 { 4225 movimento_zampa_new4(2,1,pos_braccio4_fin); //zampa4 vel 1 brac 4226 movimento_zampa_new4(1,2,pos_avambraccio4_fin); //zampa4 vel 2 avambr 4227 } 4228 4229 if(modo_altezza4 == 1)//pos < di targ 4230 { 4231 //AUMENTA ALTEZZA ZAMPA, il braccio avanza e L'AVAMBRACCIO SI APRE! E FUNZIONA 4232 //quindi se la pos_avambraccio4_fin < del targer 4233 movimento_zampa_new4(2,1,75); //zampa4 vel 1 brac 4234 movimento_zampa_new4(1,2,110); //zampa4 vel 2 avambr 4235 } //fine if(modo_altezza == 1) 4236 4237 if(modo_altezza4 == 2) 4238 { 4239 //DIMINUISCE ALTEZZA ZAMPA, il braccio indietreggia e L'AVAMBRACCIO SI CHIUDE! E FUNZIONA 4240 //quindi se la pos_avambraccio2_fin < del targer 4241 movimento_zampa_new4(2,1,20); //zampa4 vel 1 brac 4242 movimento_zampa_new4(1,2,10); //zampa4 vel 2 avambr 4243 } //fine if(modo_altezza == 2) 4244 4245timerk = 0; 4246} 4247 4248 4249 4250 4251/* 4252 //FINE CAMBIO ALTEZZA 4253 if (fine_altezz_zampa2 == 1 ) //&& fine_altezz_zampa2 == 1 && fine_altezz_zampa3 == 1 && fine_altezz_zampa4 == 1) 4254 { 4255 flag_altezza = 0; 4256 flag_altezza_serial = 0; 4257 fine_altezz_zampa1 = 0, fine_altezz_zampa2 = 0, fine_altezz_zampa3 = 0, fine_altezz_zampa4 = 0; 4258 } 4259*/ 4260 4261 4262//}// fine if (flag_altezza_serial == 1) 4263}//fine void altezza_zampe() 4264 4265void mantenimento_rotta() 4266{ 4267 4268 4269 4270 4271 4272}// fine void mantenimento_rotta() 4273 4274 4275 4276 4277int freeRam () { 4278 4279extern int __heap_start, *__brkval; 4280 4281int v; 4282 4283return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); 4284 4285} 4286 4287 4288
Downloadable files
basic electrical schea
basic electrical schea
basic electrical schea
basic electrical schea
Comments
Only logged in users can leave comments
Anonymous user
2 years ago
I respect your project. Lol!!!
aldoz
2 years ago
Thank you! :)
Anonymous user
2 years ago
I would like the code in English please. I would like a copy of the 3D print files also so I can make this Quadro Puppy. I think it is the coolest one yet.. Thanks
aldoz
2 years ago
*** UPDATE 30/4/2019 *** The robot is going to have a complete restiling of the body, legs and touch sensors. Thanks to the new 3D printed design. A new era begins for JQR despite the large delays accumulated in recent weeks (I swear, not my fault). Stay tuned, JQR is close to showing you what it is capable of;) (new pics included above)
Anonymous user
2 years ago
This is amazing!! I love it! Are you thinking of selling this or producing them to sell?
aldoz
2 years ago
In first I need to get an advanced prototype. The future can be great!
Anonymous user
2 years ago
o ,my god , the code is not english,,, save me!
aldoz
2 years ago
:) That code is REALLY old.. sorry for Italian remarks.. :\\ And if you have questions please, you are welcome.
Anonymous user
2 years ago
Hello, congratulations for what you did, it's a huge job;) I am currently making a robot walker on 4 legs but I can't see where to start. Obviously you have to do reverse kinematics but I don't see how to make the robot work by helping it with an accelerometer/gyroscope. Could you give me some ideas?
Anonymous user
2 years ago
Hello, Thank you very much for your answer :) I'm here to give you a little update on my research. I have done a lot of research and I have understood how the inverse kinematics works, which will be used to determine the general movement of the legs. I understood well why we need the gyroscope, but I'm blocking on how I'll be able to interpret the values that this sensor returns. Do you have a track on it? I know it's part of the automatic field but when I search the internet, I can't find any information on how to use the gyroscope values in the code to maintain balance. As for the robot, I finished drawing and printing it, so I will be able to move on to the calculation part.
Anonymous user
2 years ago
Thank you very much for this information, I'll dig this side there:) Did you simulate this before implementing it? (mathlab, mathematica or others)
Anonymous user
2 years ago
All right, thank you very much, I'll look into it :)
aldoz
2 years ago
I used SketchUP to simulate general walking methods/issues. but I did not have a project already written. More a day by day work.
aldoz
2 years ago
Hi again, the Gyro give you raw values. You need to use the right code to construe these raw values to obtain a XYZ values and to obtain accellerometer XYZ too. I am not using inverse kinematics as you can read about it. I have a my personal method to use the robot legs but compliment for your commitment to understand this matter.
aldoz
2 years ago
Hi! Thx for compliments :) In first you need to build a right dimension robot. You need right lenght of legs / body. After that you need to think about a cat while he is walking. An animal need his gyro cause he need to mantain right posture / CG while he is walking, turning, trotting, climbing, descending a sharp hill or walking on a rough terrain. So we need us robot got same sense. plus the use of tactile sensors in each foot. That can help about CG and right force leg putting on the ground! hope I give you a start point :)
Anonymous user
2 years ago
hey, i have noticed u used lidar sensor. That is accurate but at the same time quite costly, i believe. have u tried working on any other alternative for that sensor or do u think if that would be possible to use other proximity sensors like ultrasonic and IR and make the quadraped work effectively? nice project though!!!
Anonymous user
2 years ago
I think your perception module is too slow, I saw how you are using the Light Lidar to scan and get the scene in front of the quadruped. 15 seconds is a long time, and 100x100 is a really small area. You need to drop the light lidar, buy the OEM D410 (accuracy) or D430 (speed) sensors from intel (unless you want color, then use the D415 or the D435i - the i has a gyro and can help positioning for mapping). The processing on the chips of these devices will give you the point cloud and works outside as well since it uses stereo + an IR laser projector and you can drop the swivel action. You should do the processing of the cloud on something better than an arduino, like the raspberry pi. The rule of thumb is the following: arduino: does X and Y raspberry pi: does X, does Y, does Z, ... And you can hook up an arduino to the pi. Feel free to message me if you need any help.
aldoz
2 years ago
In first really sorry for late answer. Thank you for your advices! Can you send your email to me? ginolillo@hotmail.com
aldoz
2 years ago
Hi, I tried many options but in finish always same answer : or LIDAR or xTion (using ROS). Sure using ROS + xTion (+ raspberry pc) we can achieve incredible results but ROS is not so friendly to use.. . So right now I am using LIDAR with very good results! Obviously nothing compared with a terrain scan using ROS +xTion. Currently I have 100x100 pixels terrain scan (15 seconds to obtain these points)
Anonymous user
2 years ago
hey, I'm really fascinated with the way the robot has been made, but to cut some more cost, instead of using lidar sensor, would it be possible that you use the ultrasonic or ir sensor to measure the distances and apply the inverse kinematics accordingly?
aldoz
2 years ago
Hi! at this time, I am trying to use Xtion PRO LIVE to do the work of LIDAR. Previously I tested ir sensors to create a basic terrain map but unfortunately the result was not good; sure not good for my project goals. Xtion PRO live, used with ROS inside a Rasp3 board (linked to the main ARDUINO board) can do a GREAT work and generate a quite accurate terrain scan (at a very good speed!). The matter is infinite. Anyway I worked on a cross-over IR sensor system (using 3 or 4 of these sensors) to calculate and to recognize the objects in front of robot (so and so 25° wide view) ; this system was pretty cool but never finished to concentrate myself in the study of ROS and xTion PRO LIVE. Thank you so mutch for your kind words! (and sorry for my poor english)
Anonymous user
2 years ago
great work. more videos please.
aldoz
2 years ago
Some days and I will get the new 3D printed robot! stay tuned :)
Anonymous user
2 years ago
Hi Aldoz. Amazing work. Are you open to sharing the design files for this? (Namely body part .STL or whatever format) Thanks!
aldoz
2 years ago
Sure I open for any collaboration!
Anonymous user
2 years ago
Amazing project. This is something I would like to try to build following what you've shared. My question is I am alright with coding and everything related to it, but I am not really confident in my electronics skills. In your opinion is it possible to put everything together without that knowledge? Thanks for sharing.
aldoz
2 years ago
When I start this project I was out like a camel in the desert.. I had experience in 3DOF racing/flight sim platform but nothing like a quadruped autonomous legged robot.. .But yeah, lot of passion, lot of fascinating BostonDynamics videos, lot of work (4 years) and nothing is impossible. In these days very important update! stay tuned!
Anonymous user
2 years ago
Welcome to participate in our #OttoREMIXchallenge https://www.ottodiy.com/blog/challenge
aldoz
2 years ago
Thank you so mutch for request! I hope to be able to participate in the next event!
Anonymous user
2 years ago
I wanted to ask if it is capable of running too? I fell those servos aren't capable of that task, so could you please tell me what should i use since i want mine to be able to run at a decent speed. Will better servos do the task? If yes then what properties of the servo motor will i need to keep in mind like torque, etc.? And if it's not possible then is the only way left to make an actuator or buy one? Would be great if you could help! Great project!
Anonymous user
2 years ago
I am really impressed with this; for someone to make something as complicated as this by themselves is really impressive! I'd love to try something similar in the future.
aldoz
2 years ago
Thank you my friend! You are welcome! soon new videos about the current situation of the robot!
Anonymous user
2 years ago
any Update on this project ?
aldoz
2 years ago
Yeah, a little late but work is still in progress! please read the today update!
Anonymous user
2 years ago
Hello.... I'm also making same kind of project but I'm not good in coding so much. Can you please upload the code in English? Pleassssss............
aldoz
2 years ago
The code here is very old. But if you have any code questions, don't hesitate and ask!
Anonymous user
2 years ago
if i want to learn from you what i must do ?? i really really apreciate what are you did !big respect
Anonymous user
2 years ago
if our university send you an invitation to present your project you would accept ?
Anonymous user
2 years ago
it seems that i will do alot of research well its a big challenge for me and i'm ready and thnks very much i will ask help for sure haha thnks again for your advice !
Anonymous user
2 years ago
thnks alot i apprciate it very much i wish that one day i come to this level !
aldoz
2 years ago
When you got the right principles of mammal movements so you need to replicate the body and the legs. Obviously it's a very complex task.. but so and so you need to replicate a mammal body and legs. When you start to build or design your robot you will surround by a lot of questions! :D just start to answer to these questions. And you got your first prototype. If you need help just ask :)
aldoz
2 years ago
Start to study mammal behaviours :) then you will get some intuitions and ideas
aldoz
2 years ago
Hi again, please send me a message to ginolillo@hotmail.com
Anonymous user
2 years ago
Sorry, but can you give me the length of the legs please? Thank you so much
Anonymous user
2 years ago
OMG! this is actually amazing and has made me want to create things like this myself. Can you tell me what i would need to learn etc thank you!
aldoz
2 years ago
Thanks for compliments! :) Have you previous robotic experiences?
Anonymous user
2 years ago
Wow this is great! Molto bene!
aldoz
2 years ago
Thank you so mutch!
Anonymous user
2 years ago
hi, sir, I am interested in this project can you pls contact me through Gmail(sukesh0802@gmail.com
Anonymous user
2 years ago
How can I learn how to do things this advanced? I have figured out how to make an AI unit that is about 7 feet tall, has two legs and 4 arms, and can follow you. It is going really well I just need help on the navigation so it will not bump into walls and a way to push a button on a breadboard with a cheap bluetooth remote. Could you help me? Thanks! (BTW your project is amazing do you have the english source code?)
aldoz
2 years ago
Hi! can you write me? aldo.campodonico@hotmail.com
Anonymous user
2 years ago
Hello, congratulations for what you did, it's really a huge job. Could I ask what kind of motor you use on the four legs?
aldoz
2 years ago
Hi Thank you for compliments! I am very close to put new videos of JQR! Battery + PS2 joypad controllers. About the motors : https://hobbyking.com/it_it/bms-620mg-high-torque-servo-metal-gear-9-1kg-15sec-50g.html
Anonymous user
4 years ago
I would like the code in English please. I would like a copy of the 3D print files also so I can make this Quadro Puppy. I think it is the coolest one yet.. Thanks
minhkhanhpt
4 years ago
Sorry, but can you give me the length of the legs please? Thank you so much
Anonymous user
5 years ago
I wanted to ask if it is capable of running too? I fell those servos aren't capable of that task, so could you please tell me what should i use since i want mine to be able to run at a decent speed. Will better servos do the task? If yes then what properties of the servo motor will i need to keep in mind like torque, etc.? And if it's not possible then is the only way left to make an actuator or buy one? Would be great if you could help! Great project!
Anonymous user
5 years ago
hi, sir, I am interested in this project can you pls contact me through Gmail(sukesh0802@gmail.com
Hurry_up
5 years ago
How can I learn how to do things this advanced? I have figured out how to make an AI unit that is about 7 feet tall, has two legs and 4 arms, and can follow you. It is going really well I just need help on the navigation so it will not bump into walls and a way to push a button on a breadboard with a cheap bluetooth remote. Could you help me? Thanks! (BTW your project is amazing do you have the english source code?)
aldoz
2 years ago
Hi! can you write me? aldo.campodonico@hotmail.com
Huncholini
5 years ago
OMG! this is actually amazing and has made me want to create things like this myself. Can you tell me what i would need to learn etc thank you!
aldoz
2 years ago
Thanks for compliments! :) Have you previous robotic experiences?
kumarkhadus
6 years ago
Hello.... I'm also making same kind of project but I'm not good in coding so much. Can you please upload the code in English? Pleassssss............
aldoz
2 years ago
The code here is very old. But if you have any code questions, don't hesitate and ask!
Anonymous user
6 years ago
Welcome to participate in our #OttoREMIXchallenge https://www.ottodiy.com/blog/challenge
aldoz
2 years ago
Thank you so mutch for request! I hope to be able to participate in the next event!
Anonymous user
6 years ago
any Update on this project ?
aldoz
2 years ago
Yeah, a little late but work is still in progress! please read the today update!
Anonymous user
6 years ago
o ,my god , the code is not english,,, save me!
aldoz
2 years ago
:) That code is REALLY old.. sorry for Italian remarks.. :\\ And if you have questions please, you are welcome.
Anonymous user
6 years ago
This is amazing!! I love it! Are you thinking of selling this or producing them to sell?
aldoz
2 years ago
In first I need to get an advanced prototype. The future can be great!
Anonymous user
6 years ago
I love this project! I saw another person built a legged robot from scratch and you might find his blog where the maker posted documentation of building the robot interesting: https://burningservos.com
aldoz
2 years ago
:D no hate against that quadruped but I want more and more! ;)
Anonymous user
6 years ago
great work. more videos please.
aldoz
2 years ago
Some days and I will get the new 3D printed robot! stay tuned :)
Zanecrc
6 years ago
Hi Aldoz. Amazing work. Are you open to sharing the design files for this? (Namely body part .STL or whatever format) Thanks!
aldoz
2 years ago
Sure I open for any collaboration!
Anonymous user
6 years ago
hey, i have noticed u used lidar sensor. That is accurate but at the same time quite costly, i believe. have u tried working on any other alternative for that sensor or do u think if that would be possible to use other proximity sensors like ultrasonic and IR and make the quadraped work effectively? nice project though!!!
aldoz
2 years ago
In first really sorry for late answer. Thank you for your advices! Can you send your email to me? ginolillo@hotmail.com
aldoz
2 years ago
Hi, I tried many options but in finish always same answer : or LIDAR or xTion (using ROS). Sure using ROS + xTion (+ raspberry pc) we can achieve incredible results but ROS is not so friendly to use.. . So right now I am using LIDAR with very good results! Obviously nothing compared with a terrain scan using ROS +xTion. Currently I have 100x100 pixels terrain scan (15 seconds to obtain these points)
Anonymous user
2 years ago
I think your perception module is too slow, I saw how you are using the Light Lidar to scan and get the scene in front of the quadruped. 15 seconds is a long time, and 100x100 is a really small area. You need to drop the light lidar, buy the OEM D410 (accuracy) or D430 (speed) sensors from intel (unless you want color, then use the D415 or the D435i - the i has a gyro and can help positioning for mapping). The processing on the chips of these devices will give you the point cloud and works outside as well since it uses stereo + an IR laser projector and you can drop the swivel action. You should do the processing of the cloud on something better than an arduino, like the raspberry pi. The rule of thumb is the following: arduino: does X and Y raspberry pi: does X, does Y, does Z, ... And you can hook up an arduino to the pi. Feel free to message me if you need any help.
Anonymous user
6 years ago
if i want to learn from you what i must do ?? i really really apreciate what are you did !big respect
aldoz
2 years ago
When you got the right principles of mammal movements so you need to replicate the body and the legs. Obviously it's a very complex task.. but so and so you need to replicate a mammal body and legs. When you start to build or design your robot you will surround by a lot of questions! :D just start to answer to these questions. And you got your first prototype. If you need help just ask :)
aldoz
2 years ago
Hi again, please send me a message to ginolillo@hotmail.com
aldoz
2 years ago
Start to study mammal behaviours :) then you will get some intuitions and ideas
Anonymous user
2 years ago
if our university send you an invitation to present your project you would accept ?
Anonymous user
2 years ago
thnks alot i apprciate it very much i wish that one day i come to this level !
Anonymous user
2 years ago
it seems that i will do alot of research well its a big challenge for me and i'm ready and thnks very much i will ask help for sure haha thnks again for your advice !
aldoz
6 years ago
*** UPDATE 30/4/2019 *** The robot is going to have a complete restiling of the body, legs and touch sensors. Thanks to the new 3D printed design. A new era begins for JQR despite the large delays accumulated in recent weeks (I swear, not my fault). Stay tuned, JQR is close to showing you what it is capable of;) (new pics included above)
cyrildon
6 years ago
Hello, congratulations for what you did, it's really a huge job. Could I ask what kind of motor you use on the four legs?
aldoz
2 years ago
Hi Thank you for compliments! I am very close to put new videos of JQR! Battery + PS2 joypad controllers. About the motors : https://hobbyking.com/it_it/bms-620mg-high-torque-servo-metal-gear-9-1kg-15sec-50g.html
Anonymous user
6 years ago
I respect your project. Lol!!!
aldoz
2 years ago
Thank you! :)
Anonymous user
6 years ago
Hello, congratulations for what you did, it's a huge job;) I am currently making a robot walker on 4 legs but I can't see where to start. Obviously you have to do reverse kinematics but I don't see how to make the robot work by helping it with an accelerometer/gyroscope. Could you give me some ideas?
aldoz
2 years ago
I used SketchUP to simulate general walking methods/issues. but I did not have a project already written. More a day by day work.
Anonymous user
2 years ago
Thank you very much for this information, I'll dig this side there:) Did you simulate this before implementing it? (mathlab, mathematica or others)
Anonymous user
2 years ago
All right, thank you very much, I'll look into it :)
Anonymous user
2 years ago
Hello, Thank you very much for your answer :) I'm here to give you a little update on my research. I have done a lot of research and I have understood how the inverse kinematics works, which will be used to determine the general movement of the legs. I understood well why we need the gyroscope, but I'm blocking on how I'll be able to interpret the values that this sensor returns. Do you have a track on it? I know it's part of the automatic field but when I search the internet, I can't find any information on how to use the gyroscope values in the code to maintain balance. As for the robot, I finished drawing and printing it, so I will be able to move on to the calculation part.
aldoz
2 years ago
Hi! Thx for compliments :) In first you need to build a right dimension robot. You need right lenght of legs / body. After that you need to think about a cat while he is walking. An animal need his gyro cause he need to mantain right posture / CG while he is walking, turning, trotting, climbing, descending a sharp hill or walking on a rough terrain. So we need us robot got same sense. plus the use of tactile sensors in each foot. That can help about CG and right force leg putting on the ground! hope I give you a start point :)
aldoz
2 years ago
Hi again, the Gyro give you raw values. You need to use the right code to construe these raw values to obtain a XYZ values and to obtain accellerometer XYZ too. I am not using inverse kinematics as you can read about it. I have a my personal method to use the robot legs but compliment for your commitment to understand this matter.
Anonymous user
7 years ago
hey, I'm really fascinated with the way the robot has been made, but to cut some more cost, instead of using lidar sensor, would it be possible that you use the ultrasonic or ir sensor to measure the distances and apply the inverse kinematics accordingly?
aldoz
2 years ago
Hi! at this time, I am trying to use Xtion PRO LIVE to do the work of LIDAR. Previously I tested ir sensors to create a basic terrain map but unfortunately the result was not good; sure not good for my project goals. Xtion PRO live, used with ROS inside a Rasp3 board (linked to the main ARDUINO board) can do a GREAT work and generate a quite accurate terrain scan (at a very good speed!). The matter is infinite. Anyway I worked on a cross-over IR sensor system (using 3 or 4 of these sensors) to calculate and to recognize the objects in front of robot (so and so 25° wide view) ; this system was pretty cool but never finished to concentrate myself in the study of ROS and xTion PRO LIVE. Thank you so mutch for your kind words! (and sorry for my poor english)
malvine
7 years ago
Amazing project. This is something I would like to try to build following what you've shared. My question is I am alright with coding and everything related to it, but I am not really confident in my electronics skills. In your opinion is it possible to put everything together without that knowledge? Thanks for sharing.
aldoz
2 years ago
When I start this project I was out like a camel in the desert.. I had experience in 3DOF racing/flight sim platform but nothing like a quadruped autonomous legged robot.. .But yeah, lot of passion, lot of fascinating BostonDynamics videos, lot of work (4 years) and nothing is impossible. In these days very important update! stay tuned!
Anonymous user
7 years ago
Wow this is great! Molto bene!
aldoz
2 years ago
Thank you so mutch!
LiFu99
7 years ago
I am really impressed with this; for someone to make something as complicated as this by themselves is really impressive! I'd love to try something similar in the future.
aldoz
2 years ago
Thank you my friend! You are welcome! soon new videos about the current situation of the robot!
gillesaaa
5 months ago
I love you project ! Any update ? stl files ? Thanks