for bob
Dependencies: mbed-STM32F103C8T6_new
main.cpp
- Committer:
- hankzhang
- Date:
- 2020-08-04
- Revision:
- 14:6aebe60d1989
- Parent:
- 13:6fac0b6a946f
File content as of revision 14:6aebe60d1989:
#include "mbed.h" #include "stm32f103c8t6.h" #include "string.h" #include "main.h" #include "stdio.h" #include "stdlib.h" DigitalOut MOTOA1(PB_4); DigitalOut MOTOB1(PB_5); DigitalOut MOTOA2(PB_8); DigitalOut MOTOB2(PB_9); AnalogIn SensorCurrent(PA_0); void motor1_move(uint8_t dir); void motor2_move(uint8_t dir); void system_init(); static uint16_t cur_cnt = 0; static uint16_t g_tmp_cur_cnt = 0; uint8_t sensor_cnt,cal_cnt, cal_cnt2, tar_cnt, pre_sensor_cnt; //uint8_t dir; float sense_value; uint8_t ov_flag, init_flag, motor1_ready_flag, motor2_ready_flag, sensor_flag, ready_flag; uint8_t open_flag,close_flag; float m_val = 0; #define MAX_LENGTH_STEPS 55 #define MIN_LENGTH_STEPS 10 #define MOVING_UP 1 #define MOVING_DOWN 2 #define MOVING_FORWARD 1 #define MOVING_BACKWARD 2 #define STOP 0 //--------------------------------------------------- DigitalOut led1(PC_13); //Serial pc(PB_10,PB_11); //UARTSerial *_serial; UARTSerial debug_uart(PB_10, PB_11, 115200); UARTSerial wifi_uart(PA_2, PA_3, 115200); //Serial debug_uart(PB_10, PB_11, 115200); //Serial wifi_uart(PA_2, PA_3, 115200); InterruptIn DebugKey(PA_12); //InterruptIn DebugKey(PA_4); volatile bool button1_pressed = false; // Used in the main loop volatile bool button1_enabled = true; // Used for debouncing Timeout button1_timeout; // Used for debouncing // Enables button when bouncing is over void button1_enabled_cb(void) { button1_enabled = true; } // ISR handling button pressed event void button1_onpressed_cb(void) { if (button1_enabled) { // Disabled while the button is bouncing button1_enabled = false; button1_pressed = true; // To be read by the main loop button1_timeout.attach(callback(button1_enabled_cb), 0.3); // Debounce time 300 ms } } //Serial wifi_uart(PA_2, PA_3, 115200); //Serial debug_uart(PB_10, PB_11, 115200); char rxBuf[32]; char wifi_rxBuf[32]; //receive msg from xiaomi cloud short g_isCloud = 0; //flag for connected xiaomi cloud //char set_property[] = {"down set_properties "} //char get_property[] = {"down get_properties "} int i = 0; //--------------------------------------------------- ATCmdParser *_parser; void sensor_capture_cb(void){ char len[15]; sensor_cnt++; if(open_flag){ if(cur_cnt<255){ cur_cnt++; } } if(close_flag){ if(cur_cnt>0){ cur_cnt--; } } } void Power_thread(){/*detect current*/ char len[50]; char i = 0; while(true){ Thread::wait(200); /*unit millisec*/ sense_value = SensorCurrent.read(); if((sense_value>0.5)&&sensor_flag){ sprintf(len, "Power_thread: sense_value = %0.4f > 0.5 \r\n", sense_value); debug_uart.write(len, sizeof(len)); i++; if(i>1){ ov_flag = 1; } }else{ i = 0; } } } void Motor1_thread(){ char len[50]; while(true){ Thread::wait(300); /*unit millisec*/ if(!init_flag){ while(1){ if(motor2_ready_flag){break;}else{ wait(1); debug_uart.write("Motor1_thread: -------------------------------\r\n",48); } } motor2_ready_flag = 0; sensor_cnt = 0; motor1_move(MOVING_FORWARD); wait(1); //sensor_flag = 1; /*enable current monitoring*/ while(!ov_flag){ debug_uart.write("Motor1_thread: waiting for ov_flag = 1 \r\n",41); wait_ms(100); } motor1_move(STOP); ov_flag = 0; motor2_ready_flag = 0; cal_cnt = sensor_cnt; sprintf(len, "Motor1_thread: primary calibrated cnt is %d \r\n", cal_cnt); debug_uart.write(len, sizeof(len)); wait(1); motor1_ready_flag = 1; while(1){ if(motor2_ready_flag){break;}else{ wait(1); debug_uart.write("Motor1_thread: -------------------------------\r\n",48); } } motor2_ready_flag = 0; sensor_cnt = 0; motor1_move(MOVING_BACKWARD); while(1){ //debug_uart.write("Motor1_thread: waiting for ov_flag = 1 \r\n",41); wait_ms(100); if(ov_flag){ break; } } motor1_move(STOP); cal_cnt2 = sensor_cnt; sensor_cnt = 0; cal_cnt = (cal_cnt2+cal_cnt)/2; cal_cnt -= 5; sprintf(len, "Motor1_thread: final calibrated cnt is %d \r\n", cal_cnt); debug_uart.write(len, sizeof(len)); wait(1); motor1_ready_flag = 1; while(!init_flag){ debug_uart.write("Motor1_thread: -------------------------------\r\n",48); wait(1); } }else{ debug_uart.write("Motor1_thread: calibration done, wait for motor2 action.\r\n", 58); while(1){ if(motor2_ready_flag){break;}else{ wait(1); debug_uart.write("Motor1_thread: -------------------------------\r\n",48); } } motor2_ready_flag = 0; if(open_flag){ motor1_move(MOVING_FORWARD); while(1){ #if 0 sprintf(len, "Motor1_thread: cur_cnt is %d \r\n", cur_cnt); debug_uart.write(len, sizeof(len)); sprintf(len, "Motor1_thread: tar_cnt is %d \r\n", tar_cnt); debug_uart.write(len, sizeof(len)); debug_uart.write("Motor1_thread: **************************** \r\n", 46); wait_ms(500); #endif wait_ms(100); if(cur_cnt>=tar_cnt){ debug_uart.write("Motor1_thread: cur_cnt>=tar_cnt\r\n", 33); break; } if(ov_flag){ break; } } } if(close_flag){ motor1_move(MOVING_BACKWARD); while(1){ #if 0 sprintf(len, "Motor1_thread: cur_cnt is %d \r\n", cur_cnt); debug_uart.write(len, sizeof(len)); sprintf(len, "Motor1_thread: tar_cnt is %d \r\n", tar_cnt); debug_uart.write(len, sizeof(len)); debug_uart.write("Motor1_thread: **************************** \r\n", 46); wait(1); #endif wait_ms(100); if(cur_cnt<=tar_cnt){ debug_uart.write("Motor1_thread: cur_cnt<=tar_cnt\r\n", 33); break; } if(ov_flag){ break; } } } motor1_move(STOP); if(!ov_flag){ cur_cnt = tar_cnt; debug_uart.write("Motor1_thread: *******cur_cnt = tar_cnt******** \r\n", 49); }else{ if(open_flag){cur_cnt = cal_cnt;debug_uart.write("Motor1_thread: !!!!1\r\n", 22); } if(close_flag){cur_cnt = 0;debug_uart.write("Motor1_thread: !!!!2\r\n", 22); } } ov_flag = 0; motor1_ready_flag = 1; } } } void Motor2_thread(){ uint8_t sta1,sta2; uint8_t i; DigitalIn Stopper1(PA_13); DigitalIn Stopper2(PA_15); char len[50]; while(true){ Thread::wait(300); /*unit millisec*/ if(!init_flag){ wait(1); debug_uart.write("Motor2_thread: motor2 move up\r\n", 31); motor2_move(MOVING_UP); i = 0; while(1){ if(!Stopper1){i++;}; if(i>10){break;} } motor2_move(STOP); debug_uart.write("Motor2_thread: Up stopper triggered \r\n",38); ov_flag = 0; motor2_ready_flag = 1; wait(1); while(1){ if(motor1_ready_flag){break;}else{ wait(1);debug_uart.write("Motor2_thread: --------------------------------- \r\n",51); } } motor1_ready_flag = 0; motor2_move(MOVING_DOWN); i = 0; while(1){ if(!Stopper2){i++;}; if(i>10){break;} } motor2_move(STOP); debug_uart.write("Motor2_thread: Down stopper triggered \r\n", 40); ov_flag = 0; motor2_ready_flag = 1; while(1){ if(motor1_ready_flag){break;}else{ wait(1);debug_uart.write("Motor2_thread: --------------------------------- \r\n",51); } } motor1_ready_flag = 0; debug_uart.write("Motor2_thread: motor2 move to center\r\n",38); ov_flag = 0; motor2_move(MOVING_UP); wait(1.6); motor2_move(STOP); debug_uart.write("Motor2_thread: motor2 thread done\r\n",35); cur_cnt = 0; sensor_cnt = 0; pre_sensor_cnt = 0; init_flag = 1; }else{ debug_uart.write("Motor2_thread: calibration done.wait for open/close flag\r\n", 58); while(1){ if(open_flag){break;} if(close_flag){break;} wait(1); debug_uart.write("Motor2_thread: ------------command?------------- \r\n", 51); } g_tmp_cur_cnt = cur_cnt; if(open_flag){ debug_uart.write("Motor2_thread: --------------open--------------- \r\n", 51); i = 0; motor2_move(MOVING_UP); while(1){ if(!Stopper1){i++;}; if(i>10){break;} } motor2_move(STOP); debug_uart.write("Motor2_thread: ------------open done------------- \r\n", 51); wait(1); ov_flag = 0; motor2_ready_flag = 1; } if(close_flag){ debug_uart.write("Motor2_thread: -------------close-------------- \r\n", 50); i = 0; motor2_move(MOVING_DOWN); while(1){ if(!Stopper2){i++;}; if(i>10){break;} } motor2_move(STOP); wait(1); ov_flag = 0; motor2_ready_flag = 1; } cur_cnt = g_tmp_cur_cnt; while(1){ if(motor1_ready_flag){break;}else{ wait(1); debug_uart.write("Motor2_thread: --------------------------------- \r\n", 51); } } motor1_ready_flag = 0; debug_uart.write("Motor2_thread: motor2 move to center\r\n", 38); wait(1); ready_flag = 1; g_tmp_cur_cnt = cur_cnt; if(open_flag){ motor2_move(MOVING_DOWN); wait(1.4); } if(close_flag){ motor2_move(MOVING_UP); wait(1.6); } motor2_move(STOP); cur_cnt = g_tmp_cur_cnt; ready_flag = 0; debug_uart.write("Motor2_thread: motor2 thread done\r\n", 35); pre_sensor_cnt = sensor_cnt; open_flag = 0; close_flag = 0; } } } void led1_thread() { int length; char len[20]; char l; int position = 0; int error = 0; while (true) { wifi_uart.write("get_down\r\n", 10); if(wifi_uart.readable()) { length = wifi_uart.read(wifi_rxBuf, sizeof(wifi_rxBuf)); if(!(strncmp(wifi_rxBuf,"down none",9))) { //if return "down none" debug_uart.write("--- none\r\n",10); } else if(!(strncmp(wifi_rxBuf,"down set_properties ",20))) { //if return "down set_properties" debug_uart.write("--- set:\r\n", 10); debug_uart.write(wifi_rxBuf, length); //set properties if(wifi_rxBuf[22] == '7') { //set target-position position = atoi(&wifi_rxBuf[24]); sprintf(len, "position:%d\r\n", position); debug_uart.write(len, sizeof(len)); /*add by bob*/ if(position<=10){//0 tar_cnt = 0; }else if(position<=30){//25% tar_cnt = cal_cnt/4; }else if(position<=60){//50% tar_cnt = cal_cnt/2; }else if(position<=85){//75% tar_cnt = cal_cnt*3/4; }else{//100% tar_cnt = cal_cnt; } if(tar_cnt>cur_cnt){ open_flag = 1; debug_uart.write("open flag = 1\r\n", 15); } if(tar_cnt<cur_cnt){ close_flag = 1; debug_uart.write("close flag = 1\r\n", 16); } } //report result to cloud //wifi_uart.write("result 2 7 0\r\n", 14); #if 0 if(wifi_uart.readable()) { //length = wifi_uart.read(wifi_rxBuf, sizeof(wifi_rxBuf)); //debug_uart.write(wifi_rxBuf, length); } #endif } else if(!(strncmp(wifi_rxBuf,"down get_properties ",20))) { //if return "down get_properties" debug_uart.write("--- get:\r\n", 10); debug_uart.write(wifi_rxBuf, length); //report result to cloud } else if(!(strncmp(wifi_rxBuf,"down MIIO_net_change ",21))) { //if return "down MIIO_net_change" debug_uart.write(wifi_rxBuf, length); debug_uart.write("--- net:\r\n",10); if((!strncmp(&wifi_rxBuf[21], "offline", 7))) { //连接中 debug_uart.write("offline\r\n", 9); } else if((!strncmp(&wifi_rxBuf[21], "local", 5))) { //连上路由器但未连上服务器 debug_uart.write("local\r\n", 7); } else if((!strncmp(&wifi_rxBuf[21], "cloud", 5))) { //连上小米云服务器 debug_uart.write("cloud\r\n", 7); g_isCloud = 1; } } else { debug_uart.write(wifi_rxBuf, length); } } //thread_sleep_for(400); wait_ms(400); } } void led0_thread() { int length; while (1) { if(debug_uart.readable()) { length = debug_uart.read(rxBuf, sizeof(rxBuf)); debug_uart.write(rxBuf, length); wifi_uart.write(rxBuf, length); debug_uart.write("111\r",4); } if(wifi_uart.readable()) { length = wifi_uart.read(rxBuf, sizeof(rxBuf)); debug_uart.write(rxBuf, length); debug_uart.write("222\r",4); } wait(0.5); } } void motor1_move(uint8_t dir){/*main motor*/ #if 1 if(dir==1){/*forward*/ MOTOA1 = 0; MOTOB1 = 1; }else if(dir==2){/*backward*/ MOTOA1 = 1; MOTOB1 = 0; }else{ /*stop*/ MOTOA1 = 0; MOTOB1 = 0; } #endif } void motor2_move(uint8_t dir){/*assistant motor*/ #if 1 if(dir==1){/*up*/ MOTOA2 = 0; MOTOB2 = 1; }else if(dir==2){/*down*/ MOTOA2 = 1; MOTOB2 = 0; }else{ /*stop*/ MOTOA2 = 0; MOTOB2 = 0; } #endif } void system_init() { MOTOA1 = 0; MOTOB1 = 0; MOTOA2 = 0; MOTOB2 = 0; init_flag = 0; cur_cnt = 0; cal_cnt = 68; motor1_ready_flag = 0; motor2_ready_flag = 0; sense_value = 0; sensor_flag = 1; ready_flag = 0; debug_uart.write("*******************************\r\n",33); debug_uart.write("**********LAIWU TECH***********\r\n",33); debug_uart.write("*******************************\r\n",33); debug_uart.write("system init done, wait 3 seconds to start\r\n",43); wait(3); } int main() { int length; led1 = 1; char len[50]; debug_uart.write("hello world",11); WIFI_PWREN = 1; wait(3); system_init(); InterruptIn Hall1(PA_14); //InterruptIn Hall2(PB_3); Hall1.fall(callback(sensor_capture_cb)); // Attach ISR to handle button press event DebugKey.fall(callback(button1_onpressed_cb)); // Attach ISR to handle button press event //Hall2.fall(callback(sensor_capture_cb)); // Attach ISR to handle button press event debug_uart.write("Hall sensor init done\r\n", 23); //Thread thread0(osPriorityNormal, 512, nullptr, nullptr); Thread thread1(osPriorityNormal, 1024, nullptr, nullptr); //Thread thread2(osPriorityNormal, 512, nullptr, nullptr); Thread thread2(osPriorityNormal, 512, nullptr, nullptr); /*check the real-time current*/ //debug_uart.printf("thread1~~~~~~~~~~~~~~~~\r\n"); Thread thread3(osPriorityNormal, 1024, nullptr, nullptr); /*check the real-time current*/ //debug_uart.printf("thread2~~~~~~~~~~~~~~~~\r\n"); Thread thread4(osPriorityNormal, 1024, nullptr, nullptr); /*check the real-time current*/ //debug_uart.printf("thread3~~~~~~~~~~~~~~~~\r\n"); //thread0.start(led0_thread); thread1.start(led1_thread); thread2.start(Power_thread); thread3.start(Motor1_thread); thread4.start(Motor2_thread); debug_uart.write("four threads created\r\n",22); while(1) { if (button1_pressed) { // Set when button is pressed button1_pressed = false; debug_uart.write("change model\r\n", 14); wifi_uart.write("model laywu.curtain.test\r\n", 26); wait_ms(1000); debug_uart.write("restore wifi module\r\n",21); wifi_uart.write("restore\r\n", 9); } #if 1 if(init_flag){ if(!open_flag&&!close_flag){ if(sensor_cnt>(pre_sensor_cnt+4)){ if(cur_cnt<5) { cur_cnt = cur_cnt + (sensor_cnt - pre_sensor_cnt); tar_cnt = cal_cnt; open_flag = 1; } else { cur_cnt = cur_cnt - (sensor_cnt - pre_sensor_cnt); tar_cnt = 0; close_flag = 1; } } sprintf(len, "main_thread: sensor_cnt = %d. \r\n", sensor_cnt); debug_uart.write(len, sizeof(len)); sprintf(len, "main_thread: pre_sensor_cnt = %d. \r\n", pre_sensor_cnt); debug_uart.write(len, sizeof(len)); pre_sensor_cnt = sensor_cnt; } } #endif wait(1.5); sprintf(len, "main_thread: current cur_cnt is %d\r\n", cur_cnt); debug_uart.write(len, sizeof(len)); } }