Skip to content

Commit 90fa477

Browse files
TopgunZhTopgunZh
committed
update
1 parent 04eabdf commit 90fa477

9 files changed

Lines changed: 69 additions & 38 deletions

File tree

‎src/motion_control.c‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -71,13 +71,13 @@
7171
coord_effect2arm( &x, &y, &z ); // calculate the effect offset
7272
coord_to_angle( x, y, z,
7373
&final_angle[X_AXIS], &final_angle[Y_AXIS], &final_angle[Z_AXIS] ); // calculate final angle
74-
75-
/* char l_str[20], r_str[20], b_str[20];
74+
/*
75+
char l_str[20], r_str[20], b_str[20];
7676
dtostrf( final_angle[X_AXIS], 5, 4, l_str );
7777
dtostrf( final_angle[Y_AXIS], 5, 4, r_str );
7878
dtostrf( final_angle[Z_AXIS], 5, 4, b_str );
7979
80-
DB_PRINT_STR( "final_angle: %s, %s, %s\r\n", l_str, r_str, b_str ); */
80+
DB_PRINT_STR( "final_angle: %s, %s, %s\r\n", l_str, r_str, b_str );*/
8181

8282
if( is_angle_legal( final_angle[X_AXIS], final_angle[Y_AXIS], final_angle[Z_AXIS] ) == false ){ // check the angle
8383
//DB_PRINT_STR( "E22 position cnan't reach\n" );

‎src/protocol.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -163,7 +163,7 @@ void protocol_main_loop()
163163
if (sys.abort) { return; } // Bail to main() program loop to reset system.
164164

165165
report_parse_result();
166-
// check_motor_positon();
166+
check_motor_positon();
167167
}
168168
return; /* Never reached */
169169
}

‎src/uarm_angle.c‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -203,8 +203,8 @@ void angle_sensor_init(void){
203203
uarm.init_base_angle = calculate_current_angle(CHANNEL_BASE);
204204

205205

206-
207-
/* char l_str[20], r_str[20], b_str[20];
206+
/*
207+
char l_str[20], r_str[20], b_str[20];
208208
dtostrf( uarm.init_arml_angle, 5, 4, l_str );
209209
dtostrf( uarm.init_armr_angle, 5, 4, r_str );
210210
dtostrf( uarm.init_base_angle, 5, 4, b_str );

‎src/uarm_common.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -38,8 +38,6 @@ enum uarm_work_mode_e {
3838
#define DEFAULT_PEN_HEIGHT 43
3939
#define DEFAULT_PEN_FRONT 69.5
4040

41-
#define DEFAULT_GRIPPER_HEIGHT 51.5
42-
#define DEFAULT_GRIPPER_FRONT 105
4341

4442
struct key_param_t {
4543
enum uarm_work_mode_e work_mode;
@@ -65,6 +63,8 @@ struct uarm_state_t {
6563
volatile bool cycle_report_flag;
6664
volatile bool run_done_report_flag;
6765
volatile bool run_flag;
66+
67+
bool power_state;
6868

6969
};
7070

‎src/uarm_peripheral.c‎

Lines changed: 22 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -391,6 +391,7 @@ uint8_t get_limit_switch_status(void){
391391

392392
uint8_t get_power_status(void){
393393
uint16_t power_adc_value = getAnalogPinValue(59);
394+
// DB_PRINT_STR( "%d\r\n", power_adc_value );
394395

395396
if( power_adc_value > 100 ){
396397
return 1;
@@ -399,39 +400,43 @@ uint8_t get_power_status(void){
399400
}
400401
}
401402

402-
403-
404403
void check_motor_positon(void){
405-
if( sys.state == STATE_IDLE ){
404+
if( sys.state == STATE_IDLE && uarm.motor_state_bits == 0x0F /*&& uarm.power_state*/ ){
406405
float current_angle_l = 0, current_angle_r = 0, current_angle_b = 0;
407-
// get_current_angle( sys.position[X_AXIS], sys.position[Y_AXIS], sys.position[Z_AXIS],
408-
// &current_angle_l, &current_angle_r, &current_angle_b );
406+
409407
coord_to_angle( uarm.coord_x, uarm.coord_y, uarm.coord_z, &current_angle_l, &current_angle_r, &current_angle_b );
410408

411409
float reg_angle_l = calculate_current_angle(CHANNEL_ARML);
412410
float reg_angle_r = calculate_current_angle(CHANNEL_ARMR);
413411
float reg_angle_b = calculate_current_angle(CHANNEL_BASE) ;
414412

415413
if( fabs(current_angle_l-reg_angle_l)>0.5 || fabs(current_angle_r-reg_angle_r)>0.5 || fabs(current_angle_b-reg_angle_b+90)>0.5 ){
416-
417414
char l_str[20], r_str[20], b_str[20];
418-
dtostrf( current_angle_l, 5, 4, l_str );
419-
dtostrf( current_angle_r, 5, 4, r_str );
420-
dtostrf( current_angle_b, 5, 4, b_str );
421-
422-
DB_PRINT_STR( "angle1: %s, %s, %s\r\n", l_str, r_str, b_str );
423-
424-
dtostrf( reg_angle_l, 5, 4, l_str );
425-
dtostrf( reg_angle_r, 5, 4, r_str );
426-
dtostrf( reg_angle_b, 5, 4, b_str );
427-
428-
DB_PRINT_STR( "angle2: %s, %s, %s\r\n", l_str, r_str, b_str );
429415

416+
memset(&sys, 0, sizeof(system_t)); // Clear all system variables
417+
plan_sync_position();
418+
gc_sync_position();
419+
430420
uarm.init_arml_angle = reg_angle_l;
431421
uarm.init_armr_angle = reg_angle_r;
432422
uarm.init_base_angle = reg_angle_b;
433423
beep_tone(260, 1000);
434424

425+
uarm.target_step[X_AXIS] = sys.position[X_AXIS];
426+
uarm.target_step[Y_AXIS] = sys.position[Y_AXIS];
427+
uarm.target_step[Z_AXIS] = sys.position[Z_AXIS];
428+
429+
/* angle_to_coord( uarm.init_arml_angle, uarm.init_armr_angle, uarm.init_base_angle-90,
430+
&(uarm.coord_x), &(uarm.coord_y), &(uarm.coord_z) );
431+
432+
433+
dtostrf( uarm.coord_x, 5, 4, l_str );
434+
dtostrf( uarm.coord_y, 5, 4, r_str );
435+
dtostrf( uarm.coord_z, 5, 4, b_str );
436+
437+
DB_PRINT_STR( "coord: %s, %s, %s\r\n", l_str, r_str, b_str );*/
438+
439+
435440
float target[3] = {0};
436441
target[X_AXIS] = uarm.coord_x;
437442
target[Y_AXIS] = uarm.coord_y;

‎src/uarm_peripheral.h‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,6 @@ uint8_t get_laser_status(void);
3939
uint8_t get_limit_switch_status(void);
4040
uint8_t get_power_status(void);
4141

42-
4342
void check_motor_positon(void);
4443

4544

‎src/uarm_protocol.c‎

Lines changed: 34 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -176,17 +176,30 @@ void report_parse_result(void){
176176
cycle_report_coord();
177177
}
178178

179-
static uint8_t per_status = 0; //<! report limit sw
180-
uint8_t cur_status = get_limit_switch_status();
181-
if( cur_status != per_status ){
179+
static uint8_t limit_per_status = 0; //<! report limit sw
180+
uint8_t limit_cur_status = get_limit_switch_status();
181+
if( limit_cur_status != limit_per_status ){
182182
delay_ms(10);
183-
cur_status = get_limit_switch_status();
184-
if( cur_status != per_status ){
185-
uart_printf( "@6 N0 V%d\n", cur_status );
186-
per_status = cur_status;
183+
limit_cur_status = get_limit_switch_status();
184+
if( limit_cur_status != limit_per_status ){
185+
uart_printf( "@6 N0 V%d\n", limit_cur_status );
186+
limit_per_status = limit_cur_status;
187187
}
188188
}
189189

190+
/* static uint8_t power_per_status = 0; //<! report power
191+
uint8_t power_cur_status = get_power_status();
192+
if( power_cur_status != power_per_status ){
193+
delay_ms(10);
194+
power_cur_status = get_power_status();
195+
if( power_cur_status != power_per_status ){
196+
uart_printf( "@5 V%d\n", power_cur_status );
197+
power_per_status = power_cur_status;
198+
uarm.power_state = power_cur_status;
199+
}
200+
}*/
201+
202+
190203

191204
if( sys.state == STATE_IDLE && uarm.run_done_report_flag ){ //<! move complete report
192205
if( uarm.run_flag ){
@@ -280,6 +293,7 @@ static enum uarm_protocol_e uarm_cmd_g2202(char *payload){ // <! move motor
280293
break;
281294
case 3:
282295
servo_set_angle(angle);
296+
return UARM_CMD_OK;
283297
break;
284298
}
285299
return UARM_CMD_ERROR;
@@ -395,9 +409,21 @@ static void uarm_cmd_m17(void){ // <! lock all motor
395409
servo_init();
396410
uarm.motor_state_bits = 0x0F;
397411

398-
uarm.init_arml_angle = calculate_current_angle(CHANNEL_ARML); // <! calculate init angle
412+
memset(&sys, 0, sizeof(system_t)); // Clear all system variables
413+
plan_sync_position();
414+
gc_sync_position();
415+
416+
uarm.init_arml_angle = calculate_current_angle(CHANNEL_ARML);
399417
uarm.init_armr_angle = calculate_current_angle(CHANNEL_ARMR);
400418
uarm.init_base_angle = calculate_current_angle(CHANNEL_BASE);
419+
420+
uarm.target_step[X_AXIS] = sys.position[X_AXIS];
421+
uarm.target_step[Y_AXIS] = sys.position[Y_AXIS];
422+
uarm.target_step[Z_AXIS] = sys.position[Z_AXIS];
423+
424+
angle_to_coord( uarm.init_arml_angle, uarm.init_armr_angle, uarm.init_base_angle-90,
425+
&(uarm.coord_x), &(uarm.coord_y), &(uarm.coord_z) );
426+
401427

402428
// angle_sensor_init();
403429
}

‎src/uarm_swift.c‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@ struct uarm_state_t uarm = {0};
44
char hardware_version[8] = {0};
55

66
void uarm_swift_init(void){
7-
// DB_PRINT_STR("uarm init\r\n");
7+
//DB_PRINT_STR("uarm init\r\n");
88
delay_ms(10);
99

1010
read_hardware_version();
@@ -16,7 +16,7 @@ void uarm_swift_init(void){
1616
pump_off();
1717
gripper_relesae();
1818
laser_off();
19-
19+
2020
angle_to_coord( uarm.init_arml_angle, uarm.init_armr_angle, uarm.init_base_angle-90,
2121
&(uarm.coord_x), &(uarm.coord_y), &(uarm.coord_z) );
2222

@@ -28,6 +28,7 @@ void uarm_swift_init(void){
2828

2929
DDRG &= ~(1<<3);
3030
PORTG |= (1<<3);
31+
uarm.motor_state_bits = 0x0F;
3132

3233
printString( "@1\n" );
3334
}

‎src/uarm_swift.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,8 @@
77

88
#define DEVICE_NAME "SwiftPro"
99
#define HARDWARE_VERSION hardware_version
10-
#define SOFTWARE_VERSION "V4.1.0"
11-
#define API_VERSION "V4.0.2"
10+
#define SOFTWARE_VERSION "V4.1.1"
11+
#define API_VERSION "V4.0.1"
1212
#define BLE_UUID "FFFFFFFF"
1313

1414
void uarm_swift_init(void);

0 commit comments

Comments
 (0)