main.cpp

00001 #include "mbed.h"
00002 //#include "rtos.h"
00003 #include "stm32f103c8t6.h"
00004 #include "ATCmdParser.h"
00005 #include "UARTSerial.h"
00006 //#include "Thread.h"
00007 
00008 DigitalOut MOTOA1(PB_4);
00009 DigitalOut MOTOB1(PB_5);
00010 
00011 DigitalOut MOTOA2(PB_8);
00012 DigitalOut MOTOB2(PB_9);
00013 
00014 Serial debug_uart(PB_10, PB_11);
00015 
00016 AnalogIn SensorCurrent(PA_0);
00017 
00018 AnalogIn BatteryVoltage(PA_1);
00019 
00020 void motor1_move(uint8_t dir);
00021 void motor2_move(uint8_t dir);
00022 
00023 void system_init();
00024 
00025 UARTSerial *_serial;
00026 
00027 //Thread  thread1,thread2,thread3;
00028 
00029 #if 1
00030 
00031 uint8_t sensor_cnt,cal_cnt;
00032 uint8_t dir;
00033 float sense_value;
00034 uint8_t ov_flag, init_flag, motor1_ready_flag, motor2_ready_flag, sensor_flag;
00035 
00036 #endif
00037 
00038 void sensor_capture_cb(void){
00039     sensor_cnt++;
00040 }
00041 
00042 #define MAX_LENGTH_STEPS 55
00043 #define MIN_LENGTH_STEPS 10
00044 #define MOVING_UP 1
00045 #define MOVING_DOWN 2
00046 #define MOVING_FORWARD 1
00047 #define MOVING_BACKWARD 2
00048 #define STOP 0
00049 
00050 #if 1
00051 
00052 void Power_thread(void const *argument){/*detect current*/
00053   uint8_t i = 0;
00054   while(true){
00055     Thread::wait(500); /*unit millisec*/
00056     //debug_uart.printf("Power_thread 11111111111111\r\n");
00057     //debug_uart.printf("sensor_cnt = %d\r\n", sensor_cnt);
00058     sense_value = SensorCurrent.read(); 
00059     //debug_uart.printf("current sense_value2 =%0.4f \r\n", sense_value);
00060     if((sense_value>0.8)&&sensor_flag){
00061         debug_uart.printf("sense_value = %0.4f > 0.4 \r\n", sense_value);
00062         ov_flag = 1;
00063         //if(motor2_ready_flag){
00064             //motor2_ready_flag = 0;
00065             //while(1){motor1_move(STOP);}
00066         //}
00067     }   
00068   }
00069 }
00070 
00071 void Motor1_thread(void const *argument){/*detect current*/
00072     uint8_t i;
00073   while(true){
00074     Thread::wait(300); /*unit millisec*/
00075     if(motor2_ready_flag){   
00076         sensor_cnt = 0;
00077         motor1_move(MOVING_FORWARD);
00078         wait(1);
00079         sensor_flag = 1;
00080         while(!ov_flag){debug_uart.printf("xxxxxxxxxxxxxxxxx \r\n"); wait(1);}
00081         motor1_move(STOP);
00082         debug_uart.printf("overcurrent detected \r\n");     
00083         ov_flag = 0;
00084         motor2_ready_flag = 0;
00085         cal_cnt = sensor_cnt;
00086         debug_uart.printf("calibration done \r\n");     
00087         debug_uart.printf("calibrated cnt is %d \r\n", cal_cnt);  
00088         wait(2);
00089         debug_uart.printf("back to origianl position, motor1_ready_flag = 1\r\n"); 
00090         motor1_ready_flag = 1;
00091         while(1){
00092             if(motor2_ready_flag){break;}else{
00093                 wait(1);
00094                 debug_uart.printf("thread2----wait for motor2 ready\r\n"); //wait(1);
00095             }
00096         }
00097         motor2_ready_flag = 0;
00098         sensor_cnt = 0;
00099         sensor_flag = 0;
00100         motor1_move(MOVING_BACKWARD);
00101         debug_uart.printf("current cal_cnt is %d\r\n", cal_cnt);
00102         //while(sensor_cnt<(cal_cnt-10)){debug_uart.printf("sensor cnt is %d\r\n", sensor_cnt);}
00103         while(1){
00104             if(sensor_cnt>(cal_cnt-10)){break;}else{
00105                 wait_ms(10);
00106                 //debug_uart.printf("sensor cnt is %d \r\n", sensor_cnt); 
00107             }
00108         }
00109         debug_uart.printf("xxxxxxxxxx----------sensor cnt is %d \r\n", sensor_cnt); 
00110         //debug_uart.printf("sensor cnt is higher than equal cal_cnt - 10 \r\n"); 
00111         motor1_move(STOP);
00112         wait(2);
00113         motor1_ready_flag = 1;
00114         //if(ov_flag){ov_flag = 0;}
00115     }
00116   }
00117 }
00118 
00119 void Motor2_thread(void const *argument){/*detect current*/
00120   uint8_t sta1,sta2;
00121   DigitalIn Stopper1(PA_13);
00122   DigitalIn Stopper2(PA_15);
00123 
00124   while(true){
00125     Thread::wait(300); /*unit millisec*/
00126     if(!init_flag){
00127         #if 1
00128         wait(1);
00129         debug_uart.printf("thread 2 start \r\n");    
00130         motor2_move(MOVING_UP);
00131         while(Stopper1){;}
00132         motor2_move(STOP);
00133         debug_uart.printf("up stopper1 triggered \r\n");        
00134         //init_flag = 1;
00135         motor2_ready_flag = 1;
00136         wait(1);
00137         //while(!motor1_ready_flag){debug_uart.printf("wait for motor1 ready\r\n");wait(1);}
00138         while(1){
00139             if(motor1_ready_flag){break;}else{
00140                 wait(1);
00141                 debug_uart.printf("thread2----wait for motor1 ready \r\n"); 
00142             }
00143         }
00144         debug_uart.printf("motor1 is ready\r\n");   
00145         motor1_ready_flag = 0;
00146         motor2_move(MOVING_DOWN);
00147         while(Stopper2){;}
00148         motor2_move(STOP);
00149         debug_uart.printf("down stopper1 triggered \r\n");      
00150         motor2_ready_flag = 1;
00151         init_flag = 1;  
00152         //while(!motor1_ready_flag){debug_uart.printf("wait for finish\r\n");wait(1);}
00153         while(1){
00154             if(motor1_ready_flag){break;}else{
00155                 wait(1);
00156                 debug_uart.printf("thread2----wait for motor1 ready \r\n"); 
00157             }
00158         }
00159         debug_uart.printf("move motor2 to center\r\n"); 
00160         motor2_move(MOVING_UP);
00161         wait(1.5);
00162         motor2_move(STOP);
00163         debug_uart.printf("motor2 thread done\r\n");
00164         #endif        
00165     }
00166   }
00167 }
00168 
00169 
00170 #endif
00171 
00172 int main(){ 
00173     wait(2);
00174     
00175     system_init();  
00176     
00177     InterruptIn Hall1(PA_14);
00178     //InterruptIn Hall2(PB_3);    
00179     Hall1.fall(callback(sensor_capture_cb)); // Attach ISR to handle button press event
00180     //Hall2.fall(callback(sensor_capture_cb)); // Attach ISR to handle button press event
00181     debug_uart.printf("Hall1 init done\r\n");
00182 
00183     //Thread thread1(osPriorityNormal,2048,Power_thread, NULL); /*check the real-time current*/
00184     
00185     #if 0
00186     thread1.start(Power_thread);
00187     debug_uart.printf("thread1~~~~~~~~~~~~~~~~\r\n");
00188     thread2.start(Motor1_thread);
00189     debug_uart.printf("thread2~~~~~~~~~~~~~~~~\r\n");
00190     thread3.start(Motor2_thread);
00191     debug_uart.printf("thread3~~~~~~~~~~~~~~~~\r\n");
00192     #endif
00193     
00194     #if 1
00195     
00196     Thread thread1(Power_thread, NULL, osPriorityNormal, 2048);
00197     Thread thread2(Motor1_thread, NULL, osPriorityNormal, 2048);
00198     Thread thread3(Motor1_thread, NULL, osPriorityNormal, 2048);
00199     #endif
00200     
00201     #if 0    
00202     debug_uart.printf("thread1~~~~~~~~~~~~~~~~\r\n");
00203     Thread thread2(osPriorityNormal,2048,Motor1_thread, NULL); /*check the real-time current*/
00204     //Thread thread2(Motor1_thread, NULL, osPriorityNormal, 512); /*check the real-time current*/
00205     debug_uart.printf("thread2~~~~~~~~~~~~~~~~\r\n");
00206     Thread thread3(osPriorityNormal,2048,Motor2_thread,NULL); /*check the real-time current*/
00207     //Thread thread3(Motor2_thread, NULL, osPriorityNormal, 512); /*check the real-time current*/
00208     debug_uart.printf("thread3~~~~~~~~~~~~~~~~\r\n");
00209     #endif
00210     
00211     
00212     
00213     init_flag = 0;
00214     //motor2_ready_flag = 1;
00215     //motor2_move(MOVING_DOWN);
00216     while(1){        
00217         //debug_uart.printf("~~~~~~~~~~~~~~\r\n");
00218         wait(1);  
00219         //sense_value = SensorCurrent.read(); 
00220         debug_uart.printf("current sense_value2 =%0.4f \r\n", sense_value);        
00221     }
00222 }
00223 
00224 
00225 
00226 
00227 void motor1_move(uint8_t dir){/*main motor*/
00228   if(dir==1){/*forward*/
00229     MOTOA1 = 0;
00230     MOTOB1 = 1;
00231   }else if(dir==2){/*backward*/
00232     MOTOA1 = 1;
00233     MOTOB1 = 0;
00234   }else{ /*stop*/
00235     MOTOA1 = 0;
00236     MOTOB1 = 0;
00237   }
00238 }
00239 
00240 void motor2_move(uint8_t dir){/*assistant motor*/
00241   if(dir==1){/*up*/
00242     MOTOA2 = 0;
00243     MOTOB2 = 1;
00244   }else if(dir==2){/*down*/
00245     MOTOA2 = 1;
00246     MOTOB2 = 0;
00247   }else{ /*stop*/
00248     MOTOA2 = 0;
00249     MOTOB2 = 0;
00250   }
00251 }
00252 
00253 void system_init(){
00254   
00255   debug_uart.baud(115200);
00256   /*
00257   lcdBus.write(0x00);
00258   TFTRD = 0;
00259   TFTWR = 0;
00260   TFTDC = 0;
00261   TFTCS = 0;
00262   */
00263   MOTOA1 = 0;
00264   MOTOB1 = 0;
00265   MOTOA2 = 0;
00266   MOTOB2 = 0;
00267   init_flag = 0;
00268   motor1_ready_flag = 0;
00269   motor2_ready_flag = 0;
00270   sense_value = 0;
00271   sensor_flag = 0;
00272   
00273   debug_uart.printf("system init done\r\n");
00274  
00275 }
00276 #if 0
00277 void wifi_debug(){
00278   wifi_uart.baud(115200);
00279   debug_uart.baud(115200);
00280   WIFI_PWREN = 1;
00281   while(1){
00282     if(wifi_uart.readable()){
00283       debug_uart.putc(wifi_uart.getc());
00284     }
00285     if(debug_uart.readable()){
00286       wifi_uart.putc(debug_uart.getc());
00287     }
00288   }
00289 }
00290 void bt_debug(){
00291   bt_uart.baud(9600);
00292   debug_uart.baud(9600);
00293   while(1){
00294     if(bt_uart.readable()){
00295        debug_uart.putc(bt_uart.getc());
00296     }
00297     if(debug_uart.readable()){
00298        bt_uart.putc(debug_uart.getc());
00299     }
00300   }
00301 }
00302 void lcd_debug(){
00303   char i = 0;
00304   unsigned char str[] = "bob's test";
00305   unsigned char str2[] = "hello world";
00306   ST7789V_Init();  
00307   BlockWrite(0,COL-1,0,ROW-1); 
00308   LCD_block_test();  
00309   //DispColor(GREEN); 
00310   DispStr(str, 40, 80, BLUE, GREEN);
00311   DispStr(str2, 40, 120, BLUE, GREEN);
00312   //DispOneChar(0x32,60,60,BLUE,GREEN);
00313   while(1){
00314       ;
00315   }     
00316 }
00317 
00318 void sht20_debug(){
00319   while(1) {
00320     int temperature = sht.readTemp();
00321     debug_uart.printf("Temperature is: %d \t", temperature);
00322     int humidity=  sht.readHumidity();
00323     debug_uart.printf("Humidity: %d \r\n",humidity);
00324     wait(2);
00325   }
00326 }
00327 
00328 void eeprom_debug(){
00329   char ucdata_write[1];    
00330   if (!i2c.write((MCP24AA02_ADDR|WRITE), ucdata_write, 1, 0)){
00331   //readID(MCP24AA02_MID);
00332   //readID(MCP24AA02_DID);
00333   //Uncomment the following 6 lines of code to write Data into the EEPROM
00334   /*
00335   writeEE(0x00,0x4D); // ASCII M
00336   writeEE(0x01,0x61); // ASCII a
00337   writeEE(0x02,0x72); // ASCII r
00338   writeEE(0x03,0x74); // ASCII t
00339   writeEE(0x04,0x69); // ASCII i
00340   writeEE(0x05,0x6E); // ASCII n
00341   debug_uart.printf("\n\r");
00342   */
00343   //Uncomment the following line to Erase the EEPROM
00344   // eraseEE();
00345   #if 1
00346   for (int i=2;i<=0x7;i++){readEE(i);}
00347   debug_uart.printf("\r\n");
00348   #endif
00349   }else{
00350     debug_uart.printf("\n\rCannot get an ACK from the Device check connections!\n\r");
00351   }
00352 }
00353 
00354 #endif
00355 
00356 
00357