Código
Esquemático C / C ++
Carregar para Arduino Mega #include #include #include "BasicStepperDriver.h" #include #include // X é pitch, Y é pinos internos yawconst [] ={6,7,8,5,4,12}; // MX STEP, DIR, EN, MY STEP, DIR, ENconst int limit_switch =26, laser_pin =11, spool_pin =10, servo_pin =13, distance_trig =29, distance_echo =30; velocidade dupla =21,336; velocidade dupla =455,225; flutuar ângulo_atual =0,0; flutuar hip_distância; // distância da arma ao alvo em metros # define X_MID 164 # define Y_MID 150 # define DEADZONE 15 # define G 9.8 # define STP_PER_DEG_YAW 3.333 # define STP_PER_DEG_PITCH 184859 # define MICROSTEPS 32 # define RPM 120 # define MOTOR_STEPS_Y 200 # define MOTOR_STEPS_X 1036 / /17.7777 steps / degreesBasicStepperDriver pitch_stepper (MOTOR_STEPS_X, pins [1], pins [0]); BasicStepperDriver yaw_stepper (MOTOR_STEPS_X, pins [4], pins [3]); acionador de servo; Pixy2I2CTION, pixy; enum States {ACQUIRE; , FOGO, WIND_DOWN, RETURN}; estado estado =ACQUIRE; void setup () {Serial.begin (115200); init_pins (); atraso (1000); // home_pitch (); pixy.init (); Serial.println ("Pronto ...");} void loop () {switch (estado) {case ACQUIRE:activate_target (); estado =POSIÇÃO; digitalWrite (laser_pin, HIGH); pausa; case POSIÇÃO:Serial.println ("posicionamento"); position_gun (); estado =SPOOL; pausa; case SPOOL:Serial.println ("spool"); digitalWrite (spool_pin, HIGH); atraso (5000); estado =FOGO; pausa; case FIRE:fire_gun (); estado =WIND_DOWN; pausa; case WIND_DOWN:Serial.println ("winding down"); digitalWrite (spool_pin, LOW); atraso (2000); estado =RETORNO; digitalWrite (laser_pin, LOW); estado =ADQUIRIR; pausa; }} void fire_gun () {Serial.println ("Disparando arma!"); trigger.write (108); atraso (400); trigger.write (90); atraso (2000);} void position_gun () {float x, y; hip_distância =ping (); hip_distância / =100; while (! hip_distância) {hyp_distance =ping (); hip_distância / =100; } Serial.println (hip_distância); x =cos (ângulo_atual) * hip_distância; y =sin (ângulo_atual) * hip_distância; float target_angle =get_angle (x, y); target_angle / =100; Serial.println (ângulo_alvo); move_pitch (target_angle - current_angle); ângulo_atual =ângulo_alvo;} void adquirir_destino () {int x =0, y =0; passos longos_taken =0; bool lock =false; while (! lock) {pixy.ccc.getBlocks (); if (pixy.ccc.numBlocks) {x =pixy.ccc.blocks [0] .m_x; y =pixy.ccc.blocks [0] .m_y; Serial.print ("Alvo visto no local X:"); Serial.print (x); Serial.print (", Y:"); Serial.println (y); if (x <=(X_MID - DEADZONE)) {// Se for muito para a esquerda, mova a arma para a esquerda move_yaw (1); } else if (x> =(X_MID + DEADZONE)) {move_yaw (-1); } else if (y <=(Y_MID - DEADZONE)) {// muito para cima, mova a arma para cima pitch_stepper.move (33152); passos_tinhados + =33152; } else if (y> =(Y_MID + DEADZONE)) {pitch_stepper.move (33152); passos_tinhados + =33152; } else {lock =true; Serial.print ("Alvo bloqueado no local X:"); Serial.print (x); Serial.print (", Y:"); Serial.println (y); Serial.print ("Etapas realizadas:"); Serial.println (etapas_taken); }}} ângulo_atual =etapas_taken / STP_PER_DEG_PITCH; Serial.print ("Ângulo atual:"); Serial.println (ângulo_atual);} void init_pins () {pinMode (pinos [2], SAÍDA); pinMode (pinos [5], SAÍDA); pinMode (limit_switch, INPUT_PULLUP); pinMode (laser_pin, OUTPUT); pinMode (spool_pin, OUTPUT); pinMode (distance_echo, INPUT); pinMode (distance_trig, OUTPUT); digitalWrite (pinos [2], LOW); digitalWrite (pinos [5], LOW); digitalWrite (laser_pin, LOW); digitalWrite (spool_pin, LOW); trigger.attach (servo_pin); pitch_stepper.begin (RPM, MICROSTEPS); yaw_stepper.begin (5, MICROSTEPS); trigger.write (90);} void move_yaw (graus flutuantes) {yaw_stepper.move (graus * STP_PER_DEG_YAW * 32);} void move_pitch (graus flutuantes) {ângulo_atual + =graus; pitch_stepper.move (degrees * STP_PER_DEG_PITCH);} float get_angle (distância do float, altura do float) {float i =2 * height * 455,225; flutuante j =G * distância * distância; i + =j; j =9,8 * i; i =sqrt (pow (velocity_squared, 2) - j); return atan ((velocity_squared-i) / (G * distance)) * (180 / PI);} float ping () {Serial.println ("Obtendo distância ..."); longa duração; digitalWrite (distance_trig, LOW); atrasoMicrosegundos (5); digitalWrite (distance_trig, HIGH); atrasoMicrosegundos (10); digitalWrite (distance_trig, LOW); duração =pulseIn (distance_echo, HIGH); duração do retorno / 2 / 29,1; // distância em metros} void home_pitch () {Serial.println (digitalRead (limit_switch)); if (! digitalRead (limit_switch)) {// Se a chave estiver ativa pitch_stepper.rotate (720); } while (digitalRead (limit_switch)) {//Serial.println(digitalRead(limit_switch)); pitch_stepper.move (-32); } pitch_stepper.rotate (2880 * 2);}
