Diff: main.cpp

Revision:

1:0fe432e5dfc4

Parent:

0:217105958c2d

--- a/main.cpp	Wed Feb 20 15:13:20 2013 +0000
+++ b/main.cpp	Wed Apr 15 11:36:03 2020 +0000
@@ -1,37 +1,357 @@
 #include "mbed.h"
-#include "lcd_driver.h"
+//#include "rtos.h"
+#include "stm32f103c8t6.h"
+#include "ATCmdParser.h"
+#include "UARTSerial.h"
+//#include "Thread.h"
+
+DigitalOut MOTOA1(PB_4);
+DigitalOut MOTOB1(PB_5);
+
+DigitalOut MOTOA2(PB_8);
+DigitalOut MOTOB2(PB_9);
+
+Serial debug_uart(PB_10, PB_11);
+
+AnalogIn SensorCurrent(PA_0);
+
+AnalogIn BatteryVoltage(PA_1);
+
+void motor1_move(uint8_t dir);
+void motor2_move(uint8_t dir);
+
+void system_init();
+
+UARTSerial *_serial;
+
+//Thread  thread1,thread2,thread3;
+
+#if 1
+
+uint8_t sensor_cnt,cal_cnt;
+uint8_t dir;
+float sense_value;
+uint8_t ov_flag, init_flag, motor1_ready_flag, motor2_ready_flag, sensor_flag;
+
+#endif
+
+void sensor_capture_cb(void){
+    sensor_cnt++;
+}
+
+#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
+
+#if 1
+
+void Power_thread(void const *argument){/*detect current*/
+  uint8_t i = 0;
+  while(true){
+    Thread::wait(500); /*unit millisec*/
+    //debug_uart.printf("Power_thread 11111111111111\r\n");
+    //debug_uart.printf("sensor_cnt = %d\r\n", sensor_cnt);
+    sense_value = SensorCurrent.read(); 
+    //debug_uart.printf("current sense_value2 =%0.4f \r\n", sense_value);
+    if((sense_value>0.8)&&sensor_flag){
+        debug_uart.printf("sense_value = %0.4f > 0.4 \r\n", sense_value);
+        ov_flag = 1;
+        //if(motor2_ready_flag){
+            //motor2_ready_flag = 0;
+            //while(1){motor1_move(STOP);}
+        //}
+    }   
+  }
+}
 
-DigitalOut myled(LED1);
+void Motor1_thread(void const *argument){/*detect current*/
+    uint8_t i;
+  while(true){
+    Thread::wait(300); /*unit millisec*/
+    if(motor2_ready_flag){   
+        sensor_cnt = 0;
+        motor1_move(MOVING_FORWARD);
+        wait(1);
+        sensor_flag = 1;
+        while(!ov_flag){debug_uart.printf("xxxxxxxxxxxxxxxxx \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("calibrated cnt is %d \r\n", cal_cnt);  
+        wait(2);
+        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("thread2----wait for motor2 ready\r\n"); //wait(1);
+            }
+        }
+        motor2_ready_flag = 0;
+        sensor_cnt = 0;
+        sensor_flag = 0;
+        motor1_move(MOVING_BACKWARD);
+        debug_uart.printf("current cal_cnt is %d\r\n", cal_cnt);
+        //while(sensor_cnt<(cal_cnt-10)){debug_uart.printf("sensor cnt is %d\r\n", sensor_cnt);}
+        while(1){
+            if(sensor_cnt>(cal_cnt-10)){break;}else{
+                wait_ms(10);
+                //debug_uart.printf("sensor cnt is %d \r\n", sensor_cnt); 
+            }
+        }
+        debug_uart.printf("xxxxxxxxxx----------sensor cnt is %d \r\n", sensor_cnt); 
+        //debug_uart.printf("sensor cnt is higher than equal cal_cnt - 10 \r\n"); 
+        motor1_move(STOP);
+        wait(2);
+        motor1_ready_flag = 1;
+        //if(ov_flag){ov_flag = 0;}
+    }
+  }
+}
+
+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){
+        #if 1
+        wait(1);
+        debug_uart.printf("thread 2 start \r\n");    
+        motor2_move(MOVING_UP);
+        while(Stopper1){;}
+        motor2_move(STOP);
+        debug_uart.printf("up stopper1 triggered \r\n");        
+        //init_flag = 1;
+        motor2_ready_flag = 1;
+        wait(1);
+        //while(!motor1_ready_flag){debug_uart.printf("wait for motor1 ready\r\n");wait(1);}
+        while(1){
+            if(motor1_ready_flag){break;}else{
+                wait(1);
+                debug_uart.printf("thread2----wait for motor1 ready \r\n"); 
+            }
+        }
+        debug_uart.printf("motor1 is ready\r\n");   
+        motor1_ready_flag = 0;
+        motor2_move(MOVING_DOWN);
+        while(Stopper2){;}
+        motor2_move(STOP);
+        debug_uart.printf("down stopper1 triggered \r\n");      
+        motor2_ready_flag = 1;
+        init_flag = 1;  
+        //while(!motor1_ready_flag){debug_uart.printf("wait for finish\r\n");wait(1);}
+        while(1){
+            if(motor1_ready_flag){break;}else{
+                wait(1);
+                debug_uart.printf("thread2----wait for motor1 ready \r\n"); 
+            }
+        }
+        debug_uart.printf("move motor2 to center\r\n"); 
+        motor2_move(MOVING_UP);
+        wait(1.5);
+        motor2_move(STOP);
+        debug_uart.printf("motor2 thread done\r\n");
+        #endif        
+    }
+  }
+}
 
 
-int main() {
+#endif
+
+int main(){ 
+    wait(2);
     
-    Serial pc(USBTX, USBRX); // tx, rx
+    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("Hall1 init done\r\n");
 
-    Lcd_Init();
-      pc.printf("Hello mbed\n");
-  LCD_clear();
-    pc.printf("clear \n");
-    Pant(0xf800);
-    Pant(0x07e0);
-   Pant(0x001f);
-        Pant(0xffff);
-    Pant(0x0000);
-
-    while(1) {
-                Pant(0xf800);
-    Pant(0x07e0);
-   Pant(0x001f);
-        Pant(0xffff);
-    Pant(0x0000);
-#if 0
-        myled = 1;
-        wait(0.5);
-        myled = 0;
-        wait(0.5);
-#endif 
+    //Thread thread1(osPriorityNormal,2048,Power_thread, NULL); /*check the real-time current*/
+    
+    #if 0
+    thread1.start(Power_thread);
+    debug_uart.printf("thread1~~~~~~~~~~~~~~~~\r\n");
+    thread2.start(Motor1_thread);
+    debug_uart.printf("thread2~~~~~~~~~~~~~~~~\r\n");
+    thread3.start(Motor2_thread);
+    debug_uart.printf("thread3~~~~~~~~~~~~~~~~\r\n");
+    #endif
+    
+    #if 1
+    
+    Thread thread1(Power_thread, NULL, osPriorityNormal, 2048);
+    Thread thread2(Motor1_thread, NULL, osPriorityNormal, 2048);
+    Thread thread3(Motor1_thread, NULL, osPriorityNormal, 2048);
+    #endif
+    
+    #if 0    
+    debug_uart.printf("thread1~~~~~~~~~~~~~~~~\r\n");
+    Thread thread2(osPriorityNormal,2048,Motor1_thread, NULL); /*check the real-time current*/
+    //Thread thread2(Motor1_thread, NULL, osPriorityNormal, 512); /*check the real-time current*/
+    debug_uart.printf("thread2~~~~~~~~~~~~~~~~\r\n");
+    Thread thread3(osPriorityNormal,2048,Motor2_thread,NULL); /*check the real-time current*/
+    //Thread thread3(Motor2_thread, NULL, osPriorityNormal, 512); /*check the real-time current*/
+    debug_uart.printf("thread3~~~~~~~~~~~~~~~~\r\n");
+    #endif
+    
+    
+    
+    init_flag = 0;
+    //motor2_ready_flag = 1;
+    //motor2_move(MOVING_DOWN);
+    while(1){        
+        //debug_uart.printf("~~~~~~~~~~~~~~\r\n");
+        wait(1);  
+        //sense_value = SensorCurrent.read(); 
+        debug_uart.printf("current sense_value2 =%0.4f \r\n", sense_value);        
     }
 }
 
 
 
+
+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);
+  /*
+  lcdBus.write(0x00);
+  TFTRD = 0;
+  TFTWR = 0;
+  TFTDC = 0;
+  TFTCS = 0;
+  */
+  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;
+  
+  debug_uart.printf("system init done\r\n");
+ 
+}
+#if 0
+void wifi_debug(){
+  wifi_uart.baud(115200);
+  debug_uart.baud(115200);
+  WIFI_PWREN = 1;
+  while(1){
+    if(wifi_uart.readable()){
+      debug_uart.putc(wifi_uart.getc());
+    }
+    if(debug_uart.readable()){
+      wifi_uart.putc(debug_uart.getc());
+    }
+  }
+}
+void bt_debug(){
+  bt_uart.baud(9600);
+  debug_uart.baud(9600);
+  while(1){
+    if(bt_uart.readable()){
+       debug_uart.putc(bt_uart.getc());
+    }
+    if(debug_uart.readable()){
+       bt_uart.putc(debug_uart.getc());
+    }
+  }
+}
+void lcd_debug(){
+  char i = 0;
+  unsigned char str[] = "bob's test";
+  unsigned char str2[] = "hello world";
+  ST7789V_Init();  
+  BlockWrite(0,COL-1,0,ROW-1); 
+  LCD_block_test();  
+  //DispColor(GREEN); 
+  DispStr(str, 40, 80, BLUE, GREEN);
+  DispStr(str2, 40, 120, BLUE, GREEN);
+  //DispOneChar(0x32,60,60,BLUE,GREEN);
+  while(1){
+      ;
+  }     
+}
+
+void sht20_debug(){
+  while(1) {
+    int temperature = sht.readTemp();
+    debug_uart.printf("Temperature is: %d \t", temperature);
+    int humidity=  sht.readHumidity();
+    debug_uart.printf("Humidity: %d \r\n",humidity);
+    wait(2);
+  }
+}
+
+void eeprom_debug(){
+  char ucdata_write[1];    
+  if (!i2c.write((MCP24AA02_ADDR|WRITE), ucdata_write, 1, 0)){
+  //readID(MCP24AA02_MID);
+  //readID(MCP24AA02_DID);
+  //Uncomment the following 6 lines of code to write Data into the EEPROM
+  /*
+  writeEE(0x00,0x4D); // ASCII M
+  writeEE(0x01,0x61); // ASCII a
+  writeEE(0x02,0x72); // ASCII r
+  writeEE(0x03,0x74); // ASCII t
+  writeEE(0x04,0x69); // ASCII i
+  writeEE(0x05,0x6E); // ASCII n
+  debug_uart.printf("\n\r");
+  */
+  //Uncomment the following line to Erase the EEPROM
+  // eraseEE();
+  #if 1
+  for (int i=2;i<=0x7;i++){readEE(i);}
+  debug_uart.printf("\r\n");
+  #endif
+  }else{
+    debug_uart.printf("\n\rCannot get an ACK from the Device check connections!\n\r");
+  }
+}
+
+#endif
+
+
+