Automatyczny podnośnik standardowej figurki FT/HFT

Warsztat wiatrówkowy

Moderator: Moderatorzy wiatrówkowi

Awatar użytkownika
chrominek
Zarejestrowany użytkownik
Zarejestrowany użytkownik
Reactions:
Posty: 153
Rejestracja: 07 grudnia 2007, 08:16
Tematy: 10
Lokalizacja: Warszawa
Grupa: Zarejestrowani użytkownicy

Re: Automatyczny podnośnik standardowej figurki FT/HFT

Post autor: chrominek »

Przetestowane na strzelnicy - ZKS 50m. Dojechało, manewrowało i wróciło. W międzyczasie intensywnie ostrzeliwane.
Obrazek
Awatar użytkownika
chrominek
Zarejestrowany użytkownik
Zarejestrowany użytkownik
Reactions:
Posty: 153
Rejestracja: 07 grudnia 2007, 08:16
Tematy: 10
Lokalizacja: Warszawa
Grupa: Zarejestrowani użytkownicy

Re: Automatyczny podnośnik standardowej figurki FT/HFT

Post autor: chrominek »



1. Platforma z figurkami została oddzielona od platformy jezdnej. Na starej platformie zostały figurki z mechanizmem podnoszenia.
2. Powstała dedykowana platforma jezdna z dwoma kołami napędowymi i jednym kołem wleczonym, co znakomicie zwiększa manewrowość.
3. Platforma z figurkami została zamocowana na płycie jezdnej na 4 kolumnach. Pomiędzy miejsce na akumulatory i sterowanie.
4. Platforma jezdna dostała większe kółka (co już prawie pozwala na jazdę na końcu strzelnicy na Siekierkach) i nowe mocowanie kół.
5. Zostało zmienione sterowanie silnikami jezdnymi - poprzez mostek H na MOSFET-ach. Zastosowałem Monster Moto.
6. Został zmieniony system sterowania na proporcjonalny (aparatura RC) - sterowanie dwukanałowe, zasięg do 300m, czyli jest zapas mocy. Odbiornik w jeździku podłączony do arduino, odczyt sterowania per kanał (odbiornik nie ma wyjścia wspólnego sygnału)

Ta żółta szmatka to tymczasowa osłona silników i sterowania przed śrutem (kevlar), aż nie wymyślę ostatecznej formy osłony.


Widok z lotu ptaka
Obrazek


Widok boczny
Obrazek

Sterowanie
Obrazek

Stabilizator 5V, Arduino Mini i Monster Shield (mostek H na mosfetach) zamocowane na wspólnej płycie rozprowadzającej zasilania. Do Arduino dochodzą 2 przewody z odbiornika RC i wychodzi 6 przewodów sterowania silnikami napędowymi, po 3 na silnik: (włącz, kierunek i "moc" czyli PWM). Figurki jak na razie mają sterowanie autonomiczne. Odbiornik 6 kanałowy, z czego 2 wykorzystane.


Koło wleczone
Obrazek


Mocowanie koła
Obrazek

Koło zamocowane
Obrazek

Łożyskowanie koła wleczonego
Obrazek
Pręt jest zamocowany w łożyskach, a dopiero te do platformy jezdnej. Dzięki temu obraca się prawie bez oporu.

... i jego mocowanie
Obrazek


Aparaturka
Obrazek
Najtańsza aparaturka modelarska. W skład wchodzi nadajnik i odbiornik (na pierwszym zdjęciu). Sterowanie proporcjonalne 4 kanały (każda z 2 manetek przesyła położenie swoje położenie przód-tył i prawo-lewo). Wykorzystuję prawy drążek (kanały 1 i 2). Nadajnik paruje się z odbiornikiem, nadajnik i odbiornik pracują w paśmie 2.4 GHz, automatycznie dobierają sobie wolny kanał. Zasięg nadajnika kilkaset metrów.

Sterowanie - kod w Arduino. Zastosowałem bibliotekę do obsługi dokładniejszego zegara niz standardowo (dokładniej mierzę długość sygnałów sterujących, teoretycznie zegar ma teraz dokładność 0,5 mikrosekundy).
Sygnały z odbiornika odbieram na pinach A1 i A0. Piny 4,5,6,7,8,9 sterują silnikami poprzez Monster Moto. Monstermoto do zasilania silników ma własne źródło zasilania, oddzielne od platformy sterującej.

Kod: Zaznacz cały

#include <eRCaGuy_Timer2_Counter.h>

//macros
#define fastDigitalRead(p_inputRegister, bitMask) ((*p_inputRegister & bitMask) ? HIGH : LOW)
#define BRAKEVCC 0
#define CW   1
#define CCW  2
#define BRAKEGND 3
#define CS_THRESHOLD 100
#define HMIN 40

//you can change this to ANY digital or analog pin, by example: 10, 8, A0, A5, etc, 
//EXCEPT A6 and A7 (which exists on the Nano and Pro Mini, for example, and are NOT capable of digital operations)


const byte CHANNELS =2;
const byte InputPin[CHANNELS] = { A1,A0};
const int ChZero[CHANNELS] = { 1500, 1500 } ; //zero width - have to be checked by each channel
const int ChMin[CHANNELS]  = { 1094, 1081 }; //
const int ChMax[CHANNELS] =  { 1898, 1955 }; //
float chUpFactor[CHANNELS]; // Coefficient to reach 255 PWM at max
float chDownFactor[CHANNELS];// Similar
int chPwm[CHANNELS];
byte inputPinBitmask[CHANNELS];
volatile byte* pInputPinRegister[CHANNELS]; // Number of channels to read
volatile boolean dataReady[CHANNELS] = {false,false}; //pulse measured
volatile unsigned long pulseCount[CHANNELS] = { 0 , 0 };

//Monstermoto H-Bridge
int inApin[2] = {7, 4};  // INA: Clockwise input
int inBpin[2] = {8, 9}; // INB: Counter-clockwise input
int pwmpin[2] = {5, 6}; // PWM input
int statpin = 13;
int pwmMotorR;
int pwmMotorL;
const int MinMotorPwm = 20;
const int MinStep = 6;
unsigned long ach_count;
float ach_micros;
float ach_micros_old;

void setup(){
  timer2.setup();  //configure timer2
  //use INPUT_PULLUP to keep the pin from floating and jumping around when nothing is connected
  for(byte i=0;i<CHANNELS;i++){
    pinMode(InputPin[i],INPUT_PULLUP); 
    inputPinBitmask[i]=digitalPinToBitMask(InputPin[i]);
    pInputPinRegister[i]=portInputRegister(digitalPinToPort(InputPin[i]));   
  }
  configurePinChangeInterrupts();
  Serial.begin(115200);
  Serial.print(F("Begin waiting for pulses on pins ")); Serial.print(InputPin[0]);Serial.print(F(" & "));Serial.print(InputPin[1]);
  Serial.println(F(".\nData will be printed after each pulse is received."));
  pinMode(statpin, OUTPUT);
  // Initialize digital pins as outputs for MonsterMoto
  for (int i=0; i<2; i++){
    pinMode(inApin[i], OUTPUT);
    pinMode(inBpin[i], OUTPUT);
    pinMode(pwmpin[i], OUTPUT);
  }
  // Initialize braked
  for (int i=0; i<2; i++){
    digitalWrite(inApin[i], LOW);
    digitalWrite(inBpin[i], LOW);
  }
  for (byte i=0; i<CHANNELS; i++){
    chUpFactor[i]=255.0/(float)(ChMax[i]-ChZero[i]);
    chDownFactor[i]=255.0/(float)(ChZero[i]-ChMin[i]);
  }
  ach_micros_old=timer2.get_micros();
}

void loop(){
  //local variables
  static int pulseTime[CHANNELS]; //us; the most recent input signal high pulse time
  static int pulseTimeOld[CHANNELS];
  static unsigned long pulseCountsCopy[CHANNELS] = {3000,3000};
   for (byte i=0; i<CHANNELS; i++){ 
     if (dataReady[i]==true){ //if a pulse just came in on channel 1 / pin 1
       noInterrupts();
       dataReady[i] = false; //reset
       pulseCountsCopy[i] = pulseCount[i]; //0.5us units
       interrupts(); 
       pulseTime[i] = (int)(pulseCountsCopy[i]/2.0); //us
     } // dataReady
   }

 if ((abs(pulseTime[0] - pulseTimeOld[0])>MinStep)||(abs(pulseTime[1] - pulseTimeOld[1])>MinStep) ){
   if  (abs(pulseTime[0] - pulseTimeOld[0])>MinStep){
     pulseTimeOld[0]=pulseTime[0];
   }
   if (abs(pulseTime[1] - pulseTimeOld[1])>MinStep) {
     pulseTimeOld[1]=pulseTime[1];
   }
//MotorsGo
// left motor: pin1 - pin2 ; right motor: pin1 + pin2 ; pin2 is left/right and pin1 forward/back
   for( byte i=0;i<CHANNELS;i++){
     if(pulseTime[i]>=ChZero[i]){
       chPwm[i]=(int)(((float)(pulseTime[i]-ChZero[i] ))*chUpFactor[i] );
     }else{
       chPwm[i]=(int)(((float)(pulseTime[i]-ChZero[i] ))*chDownFactor[i] );
     }
     Serial.print("  chPwm[x]  ");Serial.print(chPwm[i]); 
   }
   Serial.println("");
   pwmMotorR=constrain( chPwm[0]-chPwm[1], -255 , 255 );
   pwmMotorL=constrain( chPwm[0]+chPwm[1], -255 , 255 );

   Serial.print("pwmMotorR  ");Serial.print(pwmMotorR);  
   Serial.print("   pwmMotorL  ");Serial.println(pwmMotorL);
   
   if(abs(pwmMotorR)<MinMotorPwm) pwmMotorR=0;
   if(abs(pwmMotorL)<MinMotorPwm) pwmMotorL=0;
   if(pwmMotorR<=0){
     motorGo(0, CCW, abs(pwmMotorR));     
   }else {
     motorGo(0, CW, pwmMotorR);
   }
   if(pwmMotorL<=0){
     motorGo(1, CCW, abs(pwmMotorL));     
   }else {
     motorGo(1, CW, pwmMotorL);
   }
 
 } // dataReady

} //end of loop()

void pinChangeIntISR(){
  //local variables
  static boolean chStateNew[CHANNELS] = {LOW,LOW};
  static boolean chStateOld[CHANNELS] = {LOW,LOW};
  static unsigned long tStart[CHANNELS] = {0,0}; //units of 0.5us
  static unsigned long tNow = 0; //units of 0.5us

  tNow = timer2.get_count(); //0.5us units
  for (byte i=0; i<CHANNELS; i++){
    chStateNew[i]= fastDigitalRead(pInputPinRegister[i],inputPinBitmask[i]);
    if (chStateNew[i] != chStateOld[i]){
      chStateOld[i] = chStateNew[i];
      if ( chStateNew[i] == HIGH){
        tStart[i]= tNow; //0.5us units
      }else { //pin_x_state_new == LOW
        pulseCount[i] = tNow - tStart[i]; //0.5us units
        dataReady[i] = true;
      }
    }
  }
}

//Interrupt Service Routines (ISRs) for Pin Change Interrupts
//see here: http://www.gammon.com.au/interrupts

//PCINT0_vect is for pins D8 to D13
ISR(PCINT0_vect)
{
  pinChangeIntISR();
}

//PCINT1_vect is for pins A0 to A5
ISR(PCINT1_vect)
{
  pinChangeIntISR();
}

//PCINT2_vect is for pins D0 to D7
ISR(PCINT2_vect)
{
  pinChangeIntISR();
}

void configurePinChangeInterrupts(){
  //Pin Change Interrupt Configuration see ATmega328 datasheet, ex: pgs. 73-75
  //also see: C:\Program Files (x86)\Arduino\hardware\arduino\avr\variants\standard\pins_arduino.h for the macros used here

  volatile byte* pPCMSK[CHANNELS];
  volatile byte* pPCICR[CHANNELS];
  for(byte i=0;i<CHANNELS;i++){ 
    // 1st, set flags on the proper Pin Change Mask Register (PCMSK)  
    // pointer to the proper PCMSK register
    pPCMSK[i] = (volatile byte*)digitalPinToPCMSK(InputPin[i]);
    *pPCMSK[i] |= _BV(digitalPinToPCMSKbit(InputPin[i])); 
    //2nd, set flags in the Pin Change Interrupt Control Register (PCICR)
    pPCICR[i] = (volatile byte*)digitalPinToPCICR(InputPin[i]);//pointer to PCICR
    *pPCICR[i] |= _BV(digitalPinToPCICRbit(InputPin[i])); 
  }
//  //ex: to use digital pin 8 as the INPUT_PIN:
//  //turn on PCINT0_vect Pin Change Interrupts (for pins D8 to D13); see datasheet pg. 73-75.
//  //1st, set flags on the proper Pin Change Mask Register
//  PCMSK0 = 0b00000001; //here I am setting Bit0 to a 1, to mark pin D8's pin change register as on; for pin mapping see here: http://arduino.cc/en/Hacking/PinMapping168
//  //2nd, set flags in the Pin Change Interrupt Control Register
//  PCICR |= 0b00000001; //here I am turning on the pin change vector 0 interrupt, for PCINT0_vect, by setting the right-most bit to a 1
}

void motorOff(int motor){
  // Initialize braked
  for (int i=0; i<2; i++){
    digitalWrite(inApin[i], LOW);
    digitalWrite(inBpin[i], LOW);
  }
  analogWrite(pwmpin[motor], 0);
}

/* motorGo() will set a motor going in a specific direction
 the motor will continue going in that direction, at that speed
 until told to do otherwise.
 
 motor: this should be either 0 or 1, will selet which of the two
 motors to be controlled
 
 direct: Should be between 0 and 3, with the following result
 0: Brake to VCC
 1: Clockwise
 2: CounterClockwise
 3: Brake to GND
 
 pwm: should be a value between ? and 1023, higher the number, the faster
 it'll go
 */
void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm){
  if (motor <= 1){
    if (direct <=4){
      // Set inA[motor]
      if (direct <=1) digitalWrite(inApin[motor], HIGH);
      else digitalWrite(inApin[motor], LOW);
      // Set inB[motor]
      if ((direct==0)||(direct==2)) digitalWrite(inBpin[motor], HIGH);
      else digitalWrite(inBpin[motor], LOW);
      analogWrite(pwmpin[motor], pwm);
    }
  }
}
ODPOWIEDZ Poprzedni tematNastępny temat

Wróć do „Warsztat - Wiatrówki”