我目前在我的arduino上运行这个程序..
main.ino
#include "speed_profile.h"
void setup() {
// put your setup code here, to run once:
output_pin_setup();
cli();
timer1_setup();
sei();
}
void loop()
{
//int motor_steps = 23400;//-9600; //23400 - 100%
// Accelration to use.
//int motor_acceleration = 500;//500;
// Deceleration to use.
//int motor_deceleration = 500;//500;
// Speed to use.
//int motor_speed = 1000; // 1000
init_tipper_position();
compute_speed_profile(23400, 500, 500, 1000);
}
由于某种原因没有执行init_tipper_position，看起来像这样。
void init_tipper_position()
{
digitalWrite(en_pin, HIGH);
delay(1);
digitalWrite(dir_pin, LOW);
delay(1);
while((PINB & (0 << 2)))
{
digitalWrite(step_pin, HIGH);
delayMicroseconds(100);
digitalWrite(step_pin, LOW);
delayMicroseconds(100);
}
digitalWrite(en_pin, LOW);
}
我知道由于以下原因，它确实会跳过。
1）我正在处理的硬件上没有任何反应。它的假设是 移动一直到传感器被激活，但这不会发生。
如果启用引脚很高，是否无法移动电机， 在这种情况下可以使用
我有一个逻辑分析仪， 我可以使用输出范围，这表明启用不高。
那么为什么程序跳过该部分的功能呢？ 应该注意的是，如果我从main下面删除函数，该函数是有效的。
代码中使用的所有函数都可以在.cpp文件中看到。 profile只是一个跟踪某些变量的结构。
的.cpp
#include "speed_profile.h"
volatile speed_profile profile;
ros::NodeHandle_<ArduinoHardware, 1, 2, 125, 125> nh;
//ros::NodeHandle nh;
std_msgs::Int8 status_step_count;
std_msgs::Int8 status_tipper_availability;
ros::Publisher chatter_status("tipper_status", &status_step_count);
ros::Publisher chatter_availabilty("tipper_availability", &status_tipper_availability);
volatile bool global_state = false;
int received_data = 0;
void call_back_control( const std_msgs::Empty & msg)
{
global_state = true;
received_data = (10 *23400.0)/100.0; // Converts input to motor_steps.
}
ros::Subscriber<std_msgs::Empty> sub_control("tipper_control", &call_back_control );
void output_pin_setup()
{
pinMode(en_pin, OUTPUT);
pinMode(dir_pin, OUTPUT);
pinMode(step_pin, OUTPUT);
pinMode(slot_sensor_pin,INPUT_PULLUP);
nh.initNode();
nh.advertise(chatter_status);
nh.advertise(chatter_availabilty);
nh.subscribe(sub_control);
//nh.subscribe(sub_command);
profile.current_step_position = 0;
delay(10);
nh.getHardware()->setBaud(57600);
}
void init_tipper_position()
{
digitalWrite(en_pin, HIGH);
delay(1);
digitalWrite(dir_pin, LOW);
delay(1);
while((PINB & (0 << 2)))
{
digitalWrite(step_pin, HIGH);
delayMicroseconds(100);
digitalWrite(step_pin, LOW);
delayMicroseconds(100);
}
digitalWrite(en_pin, LOW);
}
void timer1_setup()
{
// Tells what part of speed ramp we are in.
profile.run_state = STOP;
// Timer/Counter 1 in mode 4 CTC (Not running).
TCCR1B = (1 << WGM12);
// Timer/Counter 1 Output Compare A Match Interrupt enable.
TIMSK1 = (1 << OCIE1A);
}
void compute_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed)
{
while (global_state == false)
{
//do nothing
status_step_count.data = ((float)(profile.current_step_position / 23400.0)) * 100.0; //map(profile.current_step_position,0,23400,0,100); // could be expensive -- Nice flowing is only available with float
status_tipper_availability.data = 1;
chatter_status.publish( &status_step_count);
chatter_availabilty.publish(&status_tipper_availability);
nh.spinOnce();
}
digitalWrite(en_pin, HIGH);
delay(1);
signed int move_steps;
if(received_data > profile.current_step_position) // Compares whether received percentage (converted to motor_steps) is greater or smaller than current_step_position.
{
profile.dir = CCW;
//motor_steps = -motor_steps;
move_steps = profile.current_step_position - received_data;
digitalWrite(dir_pin, HIGH);
}
else
{
profile.dir = CW;
move_steps = profile.current_step_position - received_data;
digitalWrite(dir_pin, LOW);
}
delay(1);
computation_of_speed_profile(move_steps, motor_accel, motor_decel, motor_speed); // Computes constants for profile..
OCR1A = 10;
// Set Timer/Counter to divide clock by 8
TCCR1B |= ((0 << CS12) | (1 << CS11) | (0 << CS10));
while (global_state == true)
{
status_step_count.data = ((float)(profile.current_step_position / 23400.0)) * 100.0; //map(profile.current_step_position,0,23400,0,100);
status_tipper_availability.data = 0;
chatter_availabilty.publish(&status_tipper_availability);
chatter_status.publish( &status_step_count);
nh.spinOnce();
//delay(100);
}
}
ISR(TIMER1_COMPA_vect)
{
// Holds next delay period.
unsigned int new_first_step_delay;
// Remember the last step delay used when accelrating.
static int last_accel_delay;
// Counting steps when moving.
static unsigned int step_count = 0;
// Keep track of remainder from new_step-delay calculation to incrase accurancy
static unsigned int rest = 0;
OCR1A = profile.first_step_delay;
switch (profile.run_state)
{
case STOP:
step_count = 0;
global_state = false;
rest = 0;
TCCR1B &= ~((1 << CS12) | (1 << CS11) | (1 << CS10)); // Stop the timer, No clock source
break;
case ACCEL:
PORTB ^= _BV(PB3); // Toggles the step_pin
step_count++;
if (profile.dir == CCW)
{
profile.current_step_position++;
}
else
{
profile.current_step_position--;
}
profile.accel_count++;
new_first_step_delay = profile.first_step_delay - (((2 * (long)profile.first_step_delay) + rest) / (4 * profile.accel_count + 1));
rest = ((2 * (long)profile.first_step_delay) + rest) % (4 * profile.accel_count + 1);
// Chech if we should start decelration.
if (step_count >= profile.decel_start)
{
profile.accel_count = profile.decel_length;
profile.run_state = DECEL;
}
// Chech if we hitted max speed.
else if (new_first_step_delay <= profile.min_time_delay)
{
last_accel_delay = new_first_step_delay;
new_first_step_delay = profile.min_time_delay;
rest = 0;
profile.run_state = RUN;
}
break;
case RUN:
PORTB ^= _BV(PB3); // Toggles the step_pin
step_count++;
if (profile.dir == CCW)
{
profile.current_step_position++;
}
else
{
profile.current_step_position--;
}
new_first_step_delay = profile.min_time_delay;
// Chech if we should start decelration.
if (step_count >= profile.decel_start)
{
profile.accel_count = profile.decel_length;
// Start decelration with same delay as accel ended with.
new_first_step_delay = last_accel_delay;
profile.run_state = DECEL;
}
break;
case DECEL:
PORTB ^= _BV(PB3); // Toggles the step_pin
step_count++;
if (profile.dir == CCW)
{
profile.current_step_position++;
}
else
{
profile.current_step_position--;
}
profile.accel_count++;
new_first_step_delay = profile.first_step_delay - (((2 * (long)profile.first_step_delay) + rest) / (4 * profile.accel_count + 1));
rest = ((2 * (long)profile.first_step_delay) + rest) % (4 * profile.accel_count + 1);
// Check if we at last step
if (profile.accel_count >= 0)
{
digitalWrite(en_pin, !digitalRead(en_pin));
profile.run_state = STOP;
}
break;
}
profile.first_step_delay = new_first_step_delay;
}
.h文件
/*
* Contains the class concerning with calculating the proper speed profile
* for accelerating and decelerating the stepper motor.
*
*/
#ifndef speed_profile_h
#define speed_profile_h
#include <Arduino.h>
#include <ros.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Empty.h>
// Timer/Counter 1 running on 3,686MHz / 8 = 460,75kHz (2,17uS). (T1-FREQ 460750)
//#define T1_FREQ 460750
#define T1_FREQ 1382400
//! Number of (full)steps per round on stepper motor in use.
#define FSPR 1600
// Maths constants. To simplify maths when calculating in compute_speed_profile().
#define ALPHA (2*3.14159/FSPR) // 2*pi/spr
#define A_T_x100 ((long)(ALPHA*T1_FREQ*100)) // (ALPHA / T1_FREQ)*100
#define T1_FREQ_148 ((int)((T1_FREQ*0.676)/100)) // divided by 100 and scaled by 0.676
#define A_SQ (long)(ALPHA*2*10000000000) // ALPHA*2*10000000000
#define A_x20000 (int)(ALPHA*20000) // ALPHA*20000
// Speed ramp states
#define STOP 0
#define ACCEL 1
#define DECEL 2
#define RUN 3
// Pin numbering
#define en_pin 13
#define dir_pin 12
#define step_pin 11
#define slot_sensor_pin 10
// Motor direction
#define CW 0
#define CCW 1
typedef struct
{
unsigned char run_state : 3; // Determining the state of the speed profile
unsigned char dir: 1; // Determining the direction the motor has to move - Start being CCW
unsigned int first_step_delay; // Period time for the next timer_delay, determines the acceleration rate.
unsigned int decel_start; // Determines at which step the deceleration should begin.
signed int decel_length; // Set the deceleration length
signed int min_time_delay; //minium time required time_delay for achieving the wanted speed.
signed int accel_count; // counter used when computing step_delay for acceleration/decelleration.
volatile signed int current_step_position; // contains the current step_position
}speed_profile;
void compute_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed);
void computation_of_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed);
void init_tipper_position();
void timer1_setup(void);
void output_pin_setup(void);
#endif
答案 0 :(得分：3)
while((PINB & (0 << 2)))
这绝不是真的。 0移位任何数量总是为零。任何以零结尾的东西总是零，即false，因此永远不会输入循环。