Diff: main.cpp

Revision:

6:a9cc2c424cf9

Parent:

4:864bb8bde75c

--- a/main.cpp	Sat Apr 18 13:04:49 2020 +0000
+++ b/main.cpp	Tue Apr 21 10:46:19 2020 +0000
@@ -5,6 +5,151 @@
 #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();
+
+uint8_t sensor_cnt,cal_cnt, cal_cnt2, cur_cnt, tar_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;
+
+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;
+    }   
+  }
+}
+
+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);
@@ -160,5 +305,81 @@
         }
         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;
+    MOTOB1 = 1;
+  }else if(dir==2){/*backward*/
+    MOTOA1 = 1;
+    MOTOB1 = 0;
+  }else{ /*stop*/
+    MOTOA1 = 0;
+    MOTOB1 = 0;
+  }
+}
+
+void motor2_move(uint8_t dir){/*assistant motor*/
+  if(dir==1){/*up*/
+    MOTOA2 = 0;
+    MOTOB2 = 1;
+  }else if(dir==2){/*down*/
+    MOTOA2 = 1;
+    MOTOB2 = 0;
+  }else{ /*stop*/
+    MOTOA2 = 0;
+    MOTOB2 = 0;
+  }
+}
+
+void system_init(){
+  debug_uart.baud(115200);
+  MOTOA1 = 0;
+  MOTOB1 = 0;
+  MOTOA2 = 0;
+  MOTOB2 = 0;
+  init_flag = 0;
+  motor1_ready_flag = 0;
+  motor2_ready_flag = 0;
+  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");
+  wait(3);
 }
\ No newline at end of file