hank zhang
update
Dependencies: mbed-STM32F103C8T6_new
Diff: main.cpp
- Revision:
- 7:155d5b6a416f
- Parent:
- 6:a9cc2c424cf9
--- a/main.cpp Tue Apr 21 10:46:19 2020 +0000
+++ b/main.cpp Tue Apr 21 11:54:09 2020 +0000
@@ -25,131 +25,15 @@
uint8_t open_flag,close_flag;
float m_val = 0;
-void sensor_capture_cb(void){
- sensor_cnt++;
- cur_cnt++;
-}
-
-void Power_thread(){/*detect current*/
- char len[50];
- while(true){
- Thread::wait(500); /*unit millisec*/
- sense_value = SensorCurrent.read();
- if((sense_value>0.5)&&sensor_flag){
- sprintf(len, "position:%d", position);
- debug_uart.write("Power_thread: sense_value = %0.4f > 0.5 \r\n", sense_value);
- ov_flag = 1;
- }
- }
-}
+#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
-void Motor1_thread(void const *argument){/*detect current*/
- while(true){
- Thread::wait(300); /*unit millisec*/
- if(!init_flag){
- while(1){
- if(motor2_ready_flag){break;}else{
- wait(1);debug_uart.printf("Motor1_thread: --------------------------------- \r\n");
- }
- }
- motor2_ready_flag = 0;
- sensor_cnt = 0;
- motor1_move(MOVING_FORWARD);
- wait(1);
- sensor_flag = 1; /*enable current monitoring*/
- while(!ov_flag){debug_uart.printf("Motor1_thread: waiting for ov_flag = 1 \r\n"); wait(1);}
- motor1_move(STOP);
- //debug_uart.printf("overcurrent detected \r\n");
- ov_flag = 0;
- motor2_ready_flag = 0;
- cal_cnt = sensor_cnt;
- //debug_uart.printf("calibration done \r\n");
- debug_uart.printf("Motor1_thread: calibrated cnt is %d \r\n", cal_cnt);
- wait(1);
- //debug_uart.printf("back to origianl position, motor1_ready_flag = 1\r\n");
- motor1_ready_flag = 1;
- while(1){
- if(motor2_ready_flag){break;}else{
- wait(1);debug_uart.printf("Motor1_thread: --------------------------------- \r\n");
- }
- }
- motor2_ready_flag = 0;
- sensor_cnt = 0;
- //sensor_flag = 0;
- motor1_move(MOVING_BACKWARD);
- debug_uart.printf("Motor1_thread: target position is %d\r\n", cal_cnt-10);
- while(1){
- //if((sensor_cnt>(cal_cnt-5))||ov_flag){break;}else{wait_ms(10);}
- if(sensor_cnt>(cal_cnt-5)){break;}else{wait_ms(10);}
- }
- motor1_move(STOP);
- #if 0
- if(ov_flag){
- debug_uart.printf("Motor1_thread: over current happens\r\n");
- cal_cnt2 = sensor_cnt;
- debug_uart.printf("Motor1_thread: cal_cnt2 = %d\r\n", cal_cnt2);
- sensor_cnt = 0;
- ov_flag = 0;
- }else{
- debug_uart.printf("Motor1_thread: arrive position %d \r\n", sensor_cnt);
- }
- #endif
- debug_uart.printf("Motor1_thread: arrive position %d \r\n", sensor_cnt);
- wait(1);
- motor1_ready_flag = 1;
- debug_uart.printf("Motor1_thread: motor1 thread done\r\n");
- }
- }
-}
-
-void Motor2_thread(void const *argument){/*detect current*/
- uint8_t sta1,sta2;
- DigitalIn Stopper1(PA_13);
- DigitalIn Stopper2(PA_15);
-
- while(true){
- Thread::wait(300); /*unit millisec*/
- if(!init_flag){
- wait(1);
- debug_uart.printf("Motor2_thread: motor2 move up\r\n");
- motor2_move(MOVING_UP);
- while(Stopper1){;}
- motor2_move(STOP);
- debug_uart.printf("Motor2_thread: Up stopper triggered \r\n");
- motor2_ready_flag = 1;
- wait(1);
- while(1){
- if(motor1_ready_flag){break;}else{
- wait(1);debug_uart.printf("Motor2_thread: --------------------------------- \r\n");
- }
- }
- motor1_ready_flag = 0;
- motor2_move(MOVING_DOWN);
- while(Stopper2){;}
- motor2_move(STOP);
- debug_uart.printf("Motor2_thread: Down stopper triggered \r\n");
- motor2_ready_flag = 1;
- while(1){
- if(motor1_ready_flag){break;}else{
- wait(1);debug_uart.printf("Motor2_thread: --------------------------------- \r\n");
- }
- }
- debug_uart.printf("Motor2_thread: motor2 move to center\r\n");
- ov_flag = 0;
- ready_flag = 1;
- motor2_move(MOVING_UP);
- wait(2.3);
- motor2_move(STOP);
- ready_flag = 0;
- debug_uart.printf("Motor2_thread: motor2 thread done\r\n");
- debug_uart.printf("Main_thread: mean current is =%2.4f \r\n", m_val);
- init_flag = 1;
- }
- }
-}
-
-
-
+//---------------------------------------------------
DigitalOut led1(PC_13);
//Serial pc(PB_10,PB_11);
@@ -169,8 +53,156 @@
//char set_property[] = {"down set_properties "}
//char get_property[] = {"down get_properties "}
int i = 0;
+//---------------------------------------------------
ATCmdParser *_parser;
+void sensor_capture_cb(void){
+ sensor_cnt++;
+ cur_cnt++;
+}
+
+void Power_thread(){/*detect current*/
+ char len[50];
+ while(true){
+ Thread::wait(500); /*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));
+ ov_flag = 1;
+ }
+ }
+}
+
+void Motor1_thread()
+{
+ /*detect current*/
+ 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(1);
+ }
+ motor1_move(STOP);
+ //debug_uart.printf("overcurrent detected \r\n");
+ ov_flag = 0;
+ motor2_ready_flag = 0;
+ cal_cnt = sensor_cnt;
+ //debug_uart.printf("calibration done \r\n");
+ sprintf(len, "Motor1_thread: calibrated cnt is %d \r\n", cal_cnt);
+ debug_uart.write(len, sizeof(len));
+ wait(1);
+ //debug_uart.printf("back to origianl position, motor1_ready_flag = 1\r\n");
+ 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;
+ //sensor_flag = 0;
+ motor1_move(MOVING_BACKWARD);
+ sprintf(len, "Motor1_thread: target position is %d\r\n", cal_cnt-10);
+ debug_uart.write(len, sizeof(len));
+ while(1){
+ //if((sensor_cnt>(cal_cnt-5))||ov_flag){break;}else{wait_ms(10);}
+ if(sensor_cnt>(cal_cnt-5)){break;}else{wait_ms(10);}
+ }
+ motor1_move(STOP);
+ #if 0
+ if(ov_flag){
+ debug_uart.write("Motor1_thread: over current happens\r\n",37);
+ cal_cnt2 = sensor_cnt;
+ sprintf(len, "Motor1_thread: cal_cnt2 = %d\r\n", cal_cnt2);
+ debug_uart.write(len, sizeof(len));
+ sensor_cnt = 0;
+ ov_flag = 0;
+ }else{
+ sprintf(len, "Motor1_thread: arrive position %d \r\n", sensor_cnt);
+ debug_uart.write(len, sizeof(len));
+ }
+ #endif
+ sprintf(len, "Motor1_thread: arrive position %d \r\n", sensor_cnt);
+ debug_uart.write(len, sizeof(len));
+ wait(1);
+ motor1_ready_flag = 1;
+ debug_uart.write("Motor1_thread: motor1 thread done\r\n", 35);
+ }
+ }
+}
+
+void Motor2_thread()
+{
+ /*detect current*/
+ uint8_t sta1,sta2;
+ 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);
+ while(Stopper1){;}
+ motor2_move(STOP);
+ debug_uart.write("Motor2_thread: Up stopper triggered \r\n",38);
+ 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);
+ while(Stopper2){;}
+ motor2_move(STOP);
+ debug_uart.write("Motor2_thread: Down stopper triggered \r\n", 40);
+ motor2_ready_flag = 1;
+ while(1){
+ if(motor1_ready_flag){break;}else{
+ wait(1);debug_uart.write("Motor2_thread: --------------------------------- \r\n",51);
+ }
+ }
+ debug_uart.write("Motor2_thread: motor2 move to center\r\n",38);
+ ov_flag = 0;
+ ready_flag = 1;
+ motor2_move(MOVING_UP);
+ wait(2.3);
+ motor2_move(STOP);
+ ready_flag = 0;
+ debug_uart.write("Motor2_thread: motor2 thread done\r\n",35);
+ sprintf(len, "Main_thread: mean current is =%2.4f \r\n", m_val);
+ debug_uart.write(len, sizeof(len));
+ init_flag = 1;
+ }
+ }
+}
+
+
void led1_thread() {
int length;
@@ -276,69 +308,6 @@
}
-int main() {
- int length;
- led1 = 1;
-
- wait(1);
-
- debug_uart.write("hello world",11);
-
- WIFI_PWREN = 1;
-
- //Thread thread0(osPriorityNormal, 512, nullptr, nullptr);
- Thread thread1(osPriorityNormal, 512, nullptr, nullptr);
- //Thread thread2(osPriorityNormal, 512, nullptr, nullptr);
-
- //thread0.start(led0_thread);
- thread1.start(led1_thread);
-
- //wifi_serial.printf("model\r\n");
- //thread1.start(led1_thread);
- //thread2.start(led2_thread);
- while(1)
- {
- debug_uart.write("--main--\r\n",10);
- if(g_isCloud)
- {
- debug_uart.write("connect to xiaomi cloud\r\n",25);
- }
- 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
- //Hall2.fall(callback(sensor_capture_cb)); // Attach ISR to handle button press event
- debug_uart.printf("Hall sensor init done\r\n");
-
- Thread thread1(Power_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE); /*check the real-time current*/
- //debug_uart.printf("thread1~~~~~~~~~~~~~~~~\r\n");
- Thread thread2(Motor1_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE); /*check the real-time current*/
- //debug_uart.printf("thread2~~~~~~~~~~~~~~~~\r\n");
- Thread thread3(Motor2_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE); /*check the real-time current*/
- //debug_uart.printf("thread3~~~~~~~~~~~~~~~~\r\n");
-
- debug_uart.printf("three threads created\r\n");
-
- while(1){
- //debug_uart.printf("~~~~~~~~~~~~~~\r\n");
- if(ready_flag){
- m_val += sense_value;
- if(sense_value>0.5){
- motor2_move(STOP);
- }
- }
- //sense_value = SensorCurrent.read();
- //if(sense_value > 0.7){
- //debug_uart.printf("Main_thread: WARNING WARNING WARNING current data is higher than 0.7\r\n");
- //}
- }
-
-}
-
void motor1_move(uint8_t dir){/*main motor*/
if(dir==1){/*forward*/
MOTOA1 = 0;
@@ -365,8 +334,9 @@
}
}
-void system_init(){
- debug_uart.baud(115200);
+void system_init()
+{
+
MOTOA1 = 0;
MOTOB1 = 0;
MOTOA2 = 0;
@@ -377,9 +347,60 @@
sense_value = 0;
sensor_flag = 0;
ready_flag = 0;
- debug_uart.printf("*******************************\r\n");
- debug_uart.printf("**********LAIWU TECH***********\r\n");
- debug_uart.printf("*******************************\r\n");
- debug_uart.printf("system init done, wait 3 seconds to start\r\n");
+ 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;
+
+ 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
+ //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, 512, 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, 512, nullptr, nullptr); /*check the real-time current*/
+ //debug_uart.printf("thread2~~~~~~~~~~~~~~~~\r\n");
+ Thread thread4(osPriorityNormal, 512, nullptr, nullptr); /*check the real-time current*/
+ //debug_uart.printf("thread3~~~~~~~~~~~~~~~~\r\n");
+
+ debug_uart.write("four threads created\r\n",22);
+
+ //thread0.start(led0_thread);
+ thread1.start(led1_thread);
+ thread2.start(Power_thread);
+ thread3.start(Motor1_thread);
+ thread4.start(Motor2_thread);
+
+ while(1)
+ {
+ debug_uart.write("--main--\r\n",10);
+ if(g_isCloud)
+ {
+ debug_uart.write("connect to xiaomi cloud\r\n",25);
+ }
+ if(ready_flag){
+ m_val += sense_value;
+ if(sense_value>0.5){
+ motor2_move(STOP);
+ }
+ }
+ wait(3);
+ }
}
\ No newline at end of file