Shao Rui
1
Diff: main.cpp
- Revision:
- 8:95a914f962bd
- Parent:
- 7:4362ea3d5300
- Child:
- 9:bf02fd2d7a0a
--- a/main.cpp Tue Dec 10 09:23:12 2019 +0000
+++ b/main.cpp Tue Jan 07 09:23:24 2020 +0000
@@ -6,39 +6,37 @@
#include "used_leg_message.h"
#include "data_command.h"
#include "data_pc.h"
-#include "data_board.h"
#include "mode.h"
#include "control.h"
-#include "calculate.h"
-
-
+ float pef = 0; // 从CAN获得的位置量
+ float pwf = 0;
int main()
{
- pc.baud(115200);
- dianciji.baud(115200);
- //shouzhua.baud(115200);
-
- command.baud(115200);
- command.attach(&serial_command_isr);
- //sf_m_c = 0; // 主要为接收模式
-
-
+
- board.baud(115200);
- board.attach(&serial_board_isr);
- sf_m_b = 1; // 主要为发送模式
+ pc.baud(115200);
+ shouzhua.baud(9600);
+ command.baud(115200);
+ /*******shaorui add******
+ EnterMotorMode(&EF_can); // 电机位置锁定
+ send_enable = 0;
+ state=MOTOR_MODE;
+ pc.printf("Enter Motor Mode ");
+ ***************************/
+ command.attach(&serial_command_isr);
+
+ // 主要为接收模式 // 主要为发送模式
+ ef_can.frequency(1000000);
+ ef_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+ ef_rxMsg.len = 6;
+ EF_can.len = 8;
+ EF_can.id = 0x01;
- pf_can.frequency(1000000);
- pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
- pf_rxMsg.len = 6;
- PF_can.len = 8;
- PF_can.id = 0x0a;
-
- df_can.frequency(1000000);
- df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
- df_rxMsg.len = 6;
- DF_can.len = 8;
- DF_can.id = 0x0b;
+ wf_can.frequency(1000000);
+ wf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+ wf_rxMsg.len = 6;
+ WF_can.len = 8;
+ WF_can.id = 0x0b;
NVIC_SetPriority(USART1_IRQn, 3); // pc中断优先级高于board
NVIC_SetPriority(USART2_IRQn, 4);
@@ -47,48 +45,22 @@
while(1)
{
counter++;
-
- //获取AD
+ //获取AD
ad1 = AD1.read() * 3300;
ad2 = AD2.read() * 3300;
-
+ command.putc(ad1);
+ command.putc(ad2);
//////////////////////// CAN获取电机位置速度信息 //////////////////////////
- pf_can.read(pf_rxMsg);
- unpack_reply(pf_rxMsg, &a_state);
+ ef_can.read(ef_rxMsg);
+ unpack_reply(ef_rxMsg, &a_state);
wait_us(10);
- df_can.read(df_rxMsg);
- unpack_reply(df_rxMsg, &a_state);
-
- ppf = a_state.pf.p; // 从CAN获得的当前位置
- pdf = a_state.df.p;
- //====================================================================//
+ wf_can.read(wf_rxMsg);
+ unpack_reply(wf_rxMsg, &a_state);
- /////////////////////// 485获取步态，力传感器信息 //////////////////////////
- sf_m_b = 1; // 发送模式
- wait_us(200); // 等待选择引脚电压稳定
- board.printf("A1B");
- wait_ms(1); // mbed特色，需要等待数据完全发送
- sf_m_b = 0; // 接收模式
- wait_us(200); // 等待选择引脚电压稳定
- wait_ms(3); // 等待从机上传数据
- gait_decode();
- //pc.printf("A1B: %d - %d - %f - %f - %f\n\r", Gait_num_valid, Gait_now, Gait_per_now, COP_Y, COP_X);
- gait_clear();
-
- sf_m_b = 1; // 发送模式
- wait_us(200); // 等待选择引脚电压稳定
- board.printf("A2B");
- wait_ms(1); // mbed特色，需要等待数据完全发送
- sf_m_b = 0; // 接收模式
- wait_us(200); // 等待选择引脚电压稳定
- wait_ms(4); // 等待从机上传数据
- ad_decode();
- //pc.printf("A2B: %f - %f\n\r", ad_pf, ad_df);
- ad_clear();
- //====================================================================//
-
- ////////////////////////// 电机状态控制，命令发送 //////////////////////////
- control();
+ pef = a_state.ef.p; // 从CAN获得的当前位置
+ pwf = a_state.wf.p;
+
+ command_control();
if(send_enable == 1)
{
@@ -96,28 +68,25 @@
WriteAll();
//send_enable = 0;
}
- /**********************************************************************/
-
- ////////////////////////////// 输出状态信息 //////////////////////////////
- /*
- if(counter >= 20000)
- {
- //pc.printf("\n\rpf- %.3f %.3f %.3f \n\r", a_state.pf.p, a_state.pf.v, a_state.pf.t);
- //pc.printf("df- %.3f %.3f %.3f \n\r", a_state.df.p, a_state.df.v, a_state.df.t);
+ if(duoji_command==1)
+ {moveServo(1, 2000, 1000); //1s移动1号舵机至2000位置
+ printf("Move Sucessfully1 ! \n\r");
+ wait(2);
+ moveServo(1, 500, 1000); //1s移动1号舵机至500位置
+ //BaterryOut();
+ printf("Move Sucessfully2 ! \n\r");
+ wait(2);
+
+ }
+
+ //pc.printf("\n\rPpd: %f - %f\r", SP_pf, SP_df);
+ // pc.printf("\n\rC: %f - %f\r", a_control.ef.p_des, a_control.wf.p_des);
+ // pc.printf("\n\rP: %f - %f\r", a_state.ef.p, a_state.wf.p);
+ pc.printf("%.3x,%.3x\n\r", EF_can.data[0],EF_can.data[1]);
+ pc.printf("send_enbale:%.3d, p_des: %.3f\n\r", send_enable, a_control.ef.p_des);
+ wait(2);
+ pc.printf("AD: %.3f\n\r", ad2);
+ }
- counter = 0;
- }
- */
-
- //pc.printf("\n\rPpd: %f - %f\r", SP_pf, SP_df);
- pc.printf("\n\rC: %f - %f\r", a_control.pf.p_des, a_control.df.p_des);
- pc.printf("\n\rP: %f - %f\r", a_state.pf.p, a_state.df.p);
-
- //dianciji.printf(); //电刺激输出
- //shouzhua.printf(); //手抓输出
-
- //pc.printf("A1B: %d - %d - %f - %f - %f\r", Gait_num_valid_real, Gait_now_real, Gait_per_now_real, COP_Y_real, COP_X_real);
- //pc.printf("A2B: %f - %f\n\r", ad_pf_real, ad_df_real);
- /**********************************************************************/
- }
+
}
\ No newline at end of file