Skip to content

Commit 290e270

Browse files
TopgunZhTopgunZh
committed
Create 2019-03-28
1 parent 61557e5 commit 290e270

18 files changed

+564
-3998
lines changed

‎.gitattributes

100644100755
File mode changed.

‎.gitignore

100644100755
File mode changed.

‎.travis.yml

100644100755
File mode changed.

‎README.md

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,12 @@
1-
# Swift Pro Firmware V4.3.0 (Base on [Grbl v0.9j](https://github.com/grbl/grbl) )
1+
# Swift Pro Firmware V4.4.0 (Base on [Grbl v0.9j](https://github.com/grbl/grbl) )
22

33
----------
4-
## Update Summary for v4.3.0
4+
## Update Summary for v4.4.0
55

6-
* Add serial port automatic identification system
7-
* Add extern I/O Control CMD M2240,M2241,P2240,P2241
8-
* Fix Z aixs some problem
9-
* Optimize speed setting
6+
* Fix G1 motion planning
7+
* Fix step effector motion bug
8+
* Optimize step effector return funciton
9+
* Add step effector speed control
1010

1111
## Caution
1212
#### The current firmware is not perfect and will be updated periodically

‎hex/uArmPro_V4.3.0_release_20181130.hex

Lines changed: 0 additions & 3791 deletions
This file was deleted.

‎src/motion_control.c

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -128,13 +128,23 @@
128128

129129
float final_coord[3];
130130
float final_step[3];
131+
float angle_x, angle_y, angle_z;
131132
for( int i = 1; i <= divide_numbers; i++ ){
132133

133-
final_coord[X_AXIS] = current_coord[X_AXIS] + delta_coord[X_AXIS] * i;
134+
final_coord[X_AXIS] = current_coord[X_AXIS] + delta_coord[X_AXIS] * i; // <! this is effect position
134135
final_coord[Y_AXIS] = current_coord[Y_AXIS] + delta_coord[Y_AXIS] * i;
135136
final_coord[Z_AXIS] = current_coord[Z_AXIS] + delta_coord[Z_AXIS] * i;
136137

137-
coord_effect2arm( &final_coord[X_AXIS], &final_coord[Y_AXIS], &final_coord[Z_AXIS] );
138+
coord_effect2arm( &final_coord[X_AXIS], &final_coord[Y_AXIS], &final_coord[Z_AXIS] ); // <! convert the position
139+
140+
coord_to_angle( final_coord[X_AXIS], final_coord[Y_AXIS], final_coord[Z_AXIS],
141+
&angle_x, &angle_y, &angle_z ); // <! calculate final angle
142+
143+
if( is_angle_legal( angle_x, angle_y, angle_z ) == false ){ // check the angle
144+
continue;
145+
}
146+
147+
138148

139149
uarm.coord_x = final_coord[X_AXIS];
140150
uarm.coord_y = final_coord[Y_AXIS];

‎src/step_lowlevel.c

Lines changed: 223 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,223 @@
1+
#include "step_lowlevel.h"
2+
3+
enum step_state_e {
4+
SPEED_UP = 0,
5+
SPEED_CONSTANT,
6+
SPEED_DOWN,
7+
};
8+
9+
enum step_mode_e {
10+
POSITION_MODE = 0,
11+
SPEED_MODE,
12+
};
13+
14+
struct step_param_t {
15+
enum step_state_e state;
16+
enum step_mode_e mode;
17+
volatile long remain_counts;
18+
volatile long total_conuts;
19+
volatile long constant_conunts;
20+
volatile long timer_load_val;
21+
volatile long timer_min_val;
22+
volatile long n;
23+
bool is_move_done;
24+
25+
void(*positon_complete_callback)(void);
26+
void(*speed_complete_callback)(void);
27+
}step_param = {0};
28+
29+
30+
bool step_init_ll(void){
31+
DIR_OUTPUT;
32+
STEP_OUTPUT;
33+
34+
DIR_LOW;
35+
STEP_LOW;
36+
37+
memset( &step_param, 0x00, sizeof(struct step_param_t) );
38+
39+
noInterrupts();
40+
TCCR4A = 0; // <! clear register value
41+
TCCR4B = 0; // <! clear register value
42+
TCNT4 = 0;
43+
OCR4A = 1600;
44+
TCCR4B |= (1 << WGM42);
45+
TCCR4B |= ((1 << CS41) | (1 << CS40));
46+
47+
interrupts();
48+
49+
return true;
50+
}
51+
52+
bool set_steps(long steps, long timer_min_val, void(*complete_callback)(void), bool wait){
53+
if( step_param.n > 1 ){ // <! already work
54+
return false;
55+
}
56+
57+
memset( &step_param, 0x00, sizeof(struct step_param_t) );
58+
59+
step_param.mode = POSITION_MODE;
60+
step_param.total_conuts = step_param.remain_counts = abs(steps);
61+
step_param.timer_min_val = timer_min_val;
62+
63+
step_param.timer_load_val = 1600;
64+
65+
step_param.is_move_done = false;
66+
step_param.positon_complete_callback = complete_callback;
67+
68+
if(steps > 0){ // <! changed the direction
69+
DIR_LOW;
70+
}else{
71+
DIR_HIGH;
72+
}
73+
74+
step_param.n = 0;
75+
OCR4A = step_param.timer_load_val;
76+
TIMSK4 |= (1 << OCIE4A); // <! enbale timer4 interrupt
77+
78+
if( wait ){
79+
while( !step_param.is_move_done ){
80+
delay_us(100);
81+
};
82+
}
83+
84+
return true;
85+
}
86+
87+
88+
bool set_speed(int8_t direction, long timer_min_val, void(*complete_callback)(void), bool wait){
89+
if( timer_min_val == -1 ){
90+
if( step_param.state == SPEED_DOWN ){ // <! already work
91+
return false;
92+
}
93+
step_param.state = SPEED_DOWN;
94+
if( wait ){
95+
while( step_param.n > 1 ){ delay_us(100); }
96+
}
97+
}else{
98+
if( step_param.n > 1 ){ // <! already work
99+
return false;
100+
}
101+
memset( &step_param, 0x00, sizeof(struct step_param_t) );
102+
step_param.mode = SPEED_MODE;
103+
step_param.speed_complete_callback = complete_callback;
104+
if(direction > 0){ // <! changed the direction
105+
DIR_HIGH;
106+
}else{
107+
DIR_LOW;
108+
}
109+
110+
step_param.state = SPEED_UP;
111+
step_param.timer_min_val = timer_min_val;
112+
step_param.timer_load_val = 1600;
113+
step_param.n = 0;
114+
OCR4A = step_param.timer_load_val;
115+
TIMSK4 |= (1 << OCIE4A); // <! enbale timer4 interrupt
116+
if( wait ){
117+
while( step_param.state != SPEED_CONSTANT ){ delay_us(100); }
118+
}
119+
}
120+
return true;
121+
}
122+
123+
void position_interrrupt_handle(void){
124+
if( step_param.remain_counts-- ){
125+
STEP_HIGH;
126+
STEP_LOW;
127+
}else{
128+
if( step_param.positon_complete_callback != NULL ){
129+
step_param.positon_complete_callback();
130+
}
131+
TIMSK4 &= ~(1 << OCIE4A); // <! disable timer4 interrupt
132+
step_param.is_move_done = true;
133+
}
134+
135+
switch( step_param.state ){
136+
case SPEED_UP:
137+
if( step_param.remain_counts>=step_param.total_conuts/2 ){
138+
step_param.n++;
139+
step_param.timer_load_val = step_param.timer_load_val - (2 * step_param.timer_load_val) / (4 * step_param.n + 1 ); // <! Tarlor series
140+
141+
if( step_param.timer_load_val < step_param.timer_min_val ){
142+
step_param.constant_conunts = 2 * step_param.remain_counts - step_param.total_conuts;
143+
step_param.timer_load_val = step_param.timer_min_val;
144+
step_param.state = SPEED_CONSTANT;
145+
}
146+
}else{
147+
step_param.state = SPEED_DOWN; // <! remain_counts reach half total counts, it mean starting to speed down
148+
}
149+
break;
150+
case SPEED_CONSTANT:
151+
if( step_param.constant_conunts-- <= 0 ){
152+
step_param.state = SPEED_DOWN;
153+
}
154+
break;
155+
case SPEED_DOWN:
156+
if( step_param.remain_counts ){
157+
step_param.n--;
158+
step_param.timer_load_val = (step_param.timer_load_val * (4 * step_param.n + 1)) / (4 * step_param.n + 1 - 2); // <! Tarlor series
159+
}
160+
break;
161+
}
162+
OCR4A = step_param.timer_load_val;
163+
}
164+
165+
void speed_interrupt_handle(void){
166+
STEP_HIGH;
167+
STEP_LOW;
168+
169+
switch( step_param.state ){
170+
case SPEED_UP:
171+
if( step_param.timer_load_val > step_param.timer_min_val ){
172+
step_param.n++;
173+
step_param.timer_load_val = step_param.timer_load_val - (2 * step_param.timer_load_val) / (4 * step_param.n + 1 ); // <! Tarlor series
174+
}else{
175+
if( step_param.speed_complete_callback != NULL ){
176+
step_param.speed_complete_callback();
177+
step_param.speed_complete_callback = NULL;
178+
}
179+
step_param.state = SPEED_CONSTANT;
180+
}
181+
break;
182+
case SPEED_CONSTANT:
183+
184+
break;
185+
case SPEED_DOWN:
186+
if( step_param.n-- ){
187+
step_param.timer_load_val = (step_param.timer_load_val * (4 * step_param.n + 1)) / (4 * step_param.n + 1 - 2); // <! Tarlor series
188+
}else{
189+
TIMSK4 &= ~(1 << OCIE4A); // <! disable timer4 interrupt
190+
if( step_param.speed_complete_callback != NULL ){
191+
step_param.speed_complete_callback();
192+
step_param.speed_complete_callback = NULL;
193+
}
194+
}
195+
break;
196+
}
197+
198+
OCR4A = step_param.timer_load_val;
199+
}
200+
201+
bool step_terminate(void){
202+
memset( &step_param, 0x00, sizeof(struct step_param_t) );
203+
noInterrupts();
204+
TCCR4A = 0; // <! clear register value
205+
TCCR4B = 0; // <! clear register value
206+
TCNT4 = 0;
207+
OCR4A = 1600;
208+
TCCR4B |= (1 << WGM42);
209+
TCCR4B |= ((1 << CS41) | (1 << CS40));
210+
211+
interrupts();
212+
TIMSK4 &= ~(1 << OCIE4A); // <! disable timer4 interrupt
213+
return true;
214+
}
215+
216+
217+
218+
ISR(TIMER4_COMPA_vect){
219+
switch( step_param.mode ){
220+
case POSITION_MODE: position_interrrupt_handle(); break;
221+
case SPEED_MODE: speed_interrupt_handle(); break;
222+
}
223+
}

‎src/step_lowlevel.h

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
#ifndef _STEP_LOWLEVEL_H_
2+
#define _STEP_LOWLEVEL_H_
3+
4+
#include <avr/io.h>
5+
#include <avr/pgmspace.h>
6+
#include <avr/interrupt.h>
7+
#include <avr/wdt.h>
8+
#include <util/delay.h>
9+
#include <math.h>
10+
#include <inttypes.h>
11+
#include <string.h>
12+
#include <stdlib.h>
13+
#include <stdint.h>
14+
#include <stdbool.h>
15+
16+
#include <Arduino.h>
17+
18+
19+
#define STEP_LIB_VERSION 1.2.2
20+
21+
22+
#define DIR_PIN 5
23+
#define DIR_PORT PORTH
24+
#define DIR_DDR DDRH
25+
#define STEP_PIN 6
26+
#define STEP_PORT PORTH
27+
#define STEP_DDR DDRH
28+
#define ENABLE_PIN 7
29+
#define ENABLE_PORT PORTD
30+
#define ENABLE_DDR DDRD
31+
32+
#define DIR_OUTPUT DIR_DDR |= (1<<DIR_PIN)
33+
#define DIR_HIGH DIR_PORT |= (1<<DIR_PIN)
34+
#define DIR_LOW DIR_PORT &= ~(1<<DIR_PIN)
35+
#define STEP_OUTPUT STEP_DDR |= (1<<STEP_PIN)
36+
#define STEP_HIGH STEP_PORT |= (1<<STEP_PIN)
37+
#define STEP_LOW STEP_PORT &= ~(1<<STEP_PIN)
38+
#define ENABLE_OUTPUT ENABLE_DDR |= (1<<ENABLE_PIN)
39+
#define ENABLE_HIGH ENABLE_PORT |= (1<<ENABLE_PIN)
40+
#define ENABLE_LOW ENABLE_PORT &= ~(1<<ENABLE_PIN)
41+
42+
/*
43+
NOTE:
44+
the time creater is timer4, must sure you didn't use timer4 to do other things!
45+
*/
46+
bool step_init_ll(void);
47+
48+
/*
49+
* steps :the pulse num steps>0 forward,steps<0 backward
50+
* timer_min_val :the timer minimum load count 80 max
51+
* complete_callback :move done callback
52+
* wait :default is wait
53+
*/
54+
bool set_steps(long steps, long timer_min_val, void(*complete_callback)(void), bool wait);
55+
56+
57+
58+
/*
59+
* direction :direction>0 forward direction<0 backward
60+
* timer_min_val :the timer minimum load count 80 max
61+
* complete_callback :speed reach callback
62+
* wait :default is no wait
63+
*/
64+
65+
bool set_speed(int8_t direction, long timer_min_val, void(*complete_callback)(void), bool wait);
66+
67+
68+
bool step_terminate(void);
69+
70+
#endif

‎src/stepper.c

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -547,10 +547,6 @@ void stepper_init()
547547
MS2_PORT |= MS2_MASK;
548548
MS1_PORT &= (~MS1_MASK);
549549
#else
550-
// MS3=1 MS2=1 MS1=0 1/64 step
551-
/* MS3_PORT |= MS3_MASK;
552-
MS2_PORT |= MS2_MASK;
553-
MS1_PORT &= (~MS1_MASK);*/
554550
// MS3=1 MS2=0 MS1=1 1/32 step
555551
MS3_PORT |= MS3_MASK;
556552
MS2_PORT &= ~(MS2_MASK);

0 commit comments

Comments
 (0)