Re: Automatyczny podnośnik standardowej figurki FT/HFT
: 03 kwietnia 2016, 22:25
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);
}
}
}