hank zhang / Mbed OS Hank_Curtain_Sample

update

Dependencies:   mbed-STM32F103C8T6_new

main.cpp@6:a9cc2c424cf9, 2020-04-21 (annotated)

Committer:
bbw
Date:
Tue Apr 21 10:46:19 2020 +0000
Revision:
6:a9cc2c424cf9
Parent:
4:864bb8bde75c
Child:
7:155d5b6a416f
temp;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
techneo 0:217105958c2d 1 #include "mbed.h"
bbw 1:0fe432e5dfc4 2 #include "stm32f103c8t6.h"
hankzhang 3:30d61fa10b98 3 #include "string.h"
hankzhang 3:30d61fa10b98 4 #include "main.h"
hankzhang 4:864bb8bde75c 5 #include "stdio.h"
hankzhang 4:864bb8bde75c 6 #include "stdlib.h"
bbw 1:0fe432e5dfc4 7
bbw 6:a9cc2c424cf9 8 DigitalOut MOTOA1(PB_4);
bbw 6:a9cc2c424cf9 9 DigitalOut MOTOB1(PB_5);
bbw 6:a9cc2c424cf9 10
bbw 6:a9cc2c424cf9 11 DigitalOut MOTOA2(PB_8);
bbw 6:a9cc2c424cf9 12 DigitalOut MOTOB2(PB_9);
bbw 6:a9cc2c424cf9 13
bbw 6:a9cc2c424cf9 14 AnalogIn SensorCurrent(PA_0);
bbw 6:a9cc2c424cf9 15
bbw 6:a9cc2c424cf9 16 void motor1_move(uint8_t dir);
bbw 6:a9cc2c424cf9 17 void motor2_move(uint8_t dir);
bbw 6:a9cc2c424cf9 18
bbw 6:a9cc2c424cf9 19 void system_init();
bbw 6:a9cc2c424cf9 20
bbw 6:a9cc2c424cf9 21 uint8_t sensor_cnt,cal_cnt, cal_cnt2, cur_cnt, tar_cnt;
bbw 6:a9cc2c424cf9 22 //uint8_t dir;
bbw 6:a9cc2c424cf9 23 float sense_value;
bbw 6:a9cc2c424cf9 24 uint8_t ov_flag, init_flag, motor1_ready_flag, motor2_ready_flag, sensor_flag, ready_flag;
bbw 6:a9cc2c424cf9 25 uint8_t open_flag,close_flag;
bbw 6:a9cc2c424cf9 26 float m_val = 0;
bbw 6:a9cc2c424cf9 27
bbw 6:a9cc2c424cf9 28 void sensor_capture_cb(void){
bbw 6:a9cc2c424cf9 29 sensor_cnt++;
bbw 6:a9cc2c424cf9 30 cur_cnt++;
bbw 6:a9cc2c424cf9 31 }
bbw 6:a9cc2c424cf9 32
bbw 6:a9cc2c424cf9 33 void Power_thread(){/*detect current*/
bbw 6:a9cc2c424cf9 34 char len[50];
bbw 6:a9cc2c424cf9 35 while(true){
bbw 6:a9cc2c424cf9 36 Thread::wait(500); /*unit millisec*/
bbw 6:a9cc2c424cf9 37 sense_value = SensorCurrent.read();
bbw 6:a9cc2c424cf9 38 if((sense_value>0.5)&&sensor_flag){
bbw 6:a9cc2c424cf9 39 sprintf(len, "position:%d", position);
bbw 6:a9cc2c424cf9 40 debug_uart.write("Power_thread: sense_value = %0.4f > 0.5 \r\n", sense_value);
bbw 6:a9cc2c424cf9 41 ov_flag = 1;
bbw 6:a9cc2c424cf9 42 }
bbw 6:a9cc2c424cf9 43 }
bbw 6:a9cc2c424cf9 44 }
bbw 6:a9cc2c424cf9 45
bbw 6:a9cc2c424cf9 46 void Motor1_thread(void const *argument){/*detect current*/
bbw 6:a9cc2c424cf9 47 while(true){
bbw 6:a9cc2c424cf9 48 Thread::wait(300); /*unit millisec*/
bbw 6:a9cc2c424cf9 49 if(!init_flag){
bbw 6:a9cc2c424cf9 50 while(1){
bbw 6:a9cc2c424cf9 51 if(motor2_ready_flag){break;}else{
bbw 6:a9cc2c424cf9 52 wait(1);debug_uart.printf("Motor1_thread: --------------------------------- \r\n");
bbw 6:a9cc2c424cf9 53 }
bbw 6:a9cc2c424cf9 54 }
bbw 6:a9cc2c424cf9 55 motor2_ready_flag = 0;
bbw 6:a9cc2c424cf9 56 sensor_cnt = 0;
bbw 6:a9cc2c424cf9 57 motor1_move(MOVING_FORWARD);
bbw 6:a9cc2c424cf9 58 wait(1);
bbw 6:a9cc2c424cf9 59 sensor_flag = 1; /*enable current monitoring*/
bbw 6:a9cc2c424cf9 60 while(!ov_flag){debug_uart.printf("Motor1_thread: waiting for ov_flag = 1 \r\n"); wait(1);}
bbw 6:a9cc2c424cf9 61 motor1_move(STOP);
bbw 6:a9cc2c424cf9 62 //debug_uart.printf("overcurrent detected \r\n");
bbw 6:a9cc2c424cf9 63 ov_flag = 0;
bbw 6:a9cc2c424cf9 64 motor2_ready_flag = 0;
bbw 6:a9cc2c424cf9 65 cal_cnt = sensor_cnt;
bbw 6:a9cc2c424cf9 66 //debug_uart.printf("calibration done \r\n");
bbw 6:a9cc2c424cf9 67 debug_uart.printf("Motor1_thread: calibrated cnt is %d \r\n", cal_cnt);
bbw 6:a9cc2c424cf9 68 wait(1);
bbw 6:a9cc2c424cf9 69 //debug_uart.printf("back to origianl position, motor1_ready_flag = 1\r\n");
bbw 6:a9cc2c424cf9 70 motor1_ready_flag = 1;
bbw 6:a9cc2c424cf9 71 while(1){
bbw 6:a9cc2c424cf9 72 if(motor2_ready_flag){break;}else{
bbw 6:a9cc2c424cf9 73 wait(1);debug_uart.printf("Motor1_thread: --------------------------------- \r\n");
bbw 6:a9cc2c424cf9 74 }
bbw 6:a9cc2c424cf9 75 }
bbw 6:a9cc2c424cf9 76 motor2_ready_flag = 0;
bbw 6:a9cc2c424cf9 77 sensor_cnt = 0;
bbw 6:a9cc2c424cf9 78 //sensor_flag = 0;
bbw 6:a9cc2c424cf9 79 motor1_move(MOVING_BACKWARD);
bbw 6:a9cc2c424cf9 80 debug_uart.printf("Motor1_thread: target position is %d\r\n", cal_cnt-10);
bbw 6:a9cc2c424cf9 81 while(1){
bbw 6:a9cc2c424cf9 82 //if((sensor_cnt>(cal_cnt-5))||ov_flag){break;}else{wait_ms(10);}
bbw 6:a9cc2c424cf9 83 if(sensor_cnt>(cal_cnt-5)){break;}else{wait_ms(10);}
bbw 6:a9cc2c424cf9 84 }
bbw 6:a9cc2c424cf9 85 motor1_move(STOP);
bbw 6:a9cc2c424cf9 86 #if 0
bbw 6:a9cc2c424cf9 87 if(ov_flag){
bbw 6:a9cc2c424cf9 88 debug_uart.printf("Motor1_thread: over current happens\r\n");
bbw 6:a9cc2c424cf9 89 cal_cnt2 = sensor_cnt;
bbw 6:a9cc2c424cf9 90 debug_uart.printf("Motor1_thread: cal_cnt2 = %d\r\n", cal_cnt2);
bbw 6:a9cc2c424cf9 91 sensor_cnt = 0;
bbw 6:a9cc2c424cf9 92 ov_flag = 0;
bbw 6:a9cc2c424cf9 93 }else{
bbw 6:a9cc2c424cf9 94 debug_uart.printf("Motor1_thread: arrive position %d \r\n", sensor_cnt);
bbw 6:a9cc2c424cf9 95 }
bbw 6:a9cc2c424cf9 96 #endif
bbw 6:a9cc2c424cf9 97 debug_uart.printf("Motor1_thread: arrive position %d \r\n", sensor_cnt);
bbw 6:a9cc2c424cf9 98 wait(1);
bbw 6:a9cc2c424cf9 99 motor1_ready_flag = 1;
bbw 6:a9cc2c424cf9 100 debug_uart.printf("Motor1_thread: motor1 thread done\r\n");
bbw 6:a9cc2c424cf9 101 }
bbw 6:a9cc2c424cf9 102 }
bbw 6:a9cc2c424cf9 103 }
bbw 6:a9cc2c424cf9 104
bbw 6:a9cc2c424cf9 105 void Motor2_thread(void const *argument){/*detect current*/
bbw 6:a9cc2c424cf9 106 uint8_t sta1,sta2;
bbw 6:a9cc2c424cf9 107 DigitalIn Stopper1(PA_13);
bbw 6:a9cc2c424cf9 108 DigitalIn Stopper2(PA_15);
bbw 6:a9cc2c424cf9 109
bbw 6:a9cc2c424cf9 110 while(true){
bbw 6:a9cc2c424cf9 111 Thread::wait(300); /*unit millisec*/
bbw 6:a9cc2c424cf9 112 if(!init_flag){
bbw 6:a9cc2c424cf9 113 wait(1);
bbw 6:a9cc2c424cf9 114 debug_uart.printf("Motor2_thread: motor2 move up\r\n");
bbw 6:a9cc2c424cf9 115 motor2_move(MOVING_UP);
bbw 6:a9cc2c424cf9 116 while(Stopper1){;}
bbw 6:a9cc2c424cf9 117 motor2_move(STOP);
bbw 6:a9cc2c424cf9 118 debug_uart.printf("Motor2_thread: Up stopper triggered \r\n");
bbw 6:a9cc2c424cf9 119 motor2_ready_flag = 1;
bbw 6:a9cc2c424cf9 120 wait(1);
bbw 6:a9cc2c424cf9 121 while(1){
bbw 6:a9cc2c424cf9 122 if(motor1_ready_flag){break;}else{
bbw 6:a9cc2c424cf9 123 wait(1);debug_uart.printf("Motor2_thread: --------------------------------- \r\n");
bbw 6:a9cc2c424cf9 124 }
bbw 6:a9cc2c424cf9 125 }
bbw 6:a9cc2c424cf9 126 motor1_ready_flag = 0;
bbw 6:a9cc2c424cf9 127 motor2_move(MOVING_DOWN);
bbw 6:a9cc2c424cf9 128 while(Stopper2){;}
bbw 6:a9cc2c424cf9 129 motor2_move(STOP);
bbw 6:a9cc2c424cf9 130 debug_uart.printf("Motor2_thread: Down stopper triggered \r\n");
bbw 6:a9cc2c424cf9 131 motor2_ready_flag = 1;
bbw 6:a9cc2c424cf9 132 while(1){
bbw 6:a9cc2c424cf9 133 if(motor1_ready_flag){break;}else{
bbw 6:a9cc2c424cf9 134 wait(1);debug_uart.printf("Motor2_thread: --------------------------------- \r\n");
bbw 6:a9cc2c424cf9 135 }
bbw 6:a9cc2c424cf9 136 }
bbw 6:a9cc2c424cf9 137 debug_uart.printf("Motor2_thread: motor2 move to center\r\n");
bbw 6:a9cc2c424cf9 138 ov_flag = 0;
bbw 6:a9cc2c424cf9 139 ready_flag = 1;
bbw 6:a9cc2c424cf9 140 motor2_move(MOVING_UP);
bbw 6:a9cc2c424cf9 141 wait(2.3);
bbw 6:a9cc2c424cf9 142 motor2_move(STOP);
bbw 6:a9cc2c424cf9 143 ready_flag = 0;
bbw 6:a9cc2c424cf9 144 debug_uart.printf("Motor2_thread: motor2 thread done\r\n");
bbw 6:a9cc2c424cf9 145 debug_uart.printf("Main_thread: mean current is =%2.4f \r\n", m_val);
bbw 6:a9cc2c424cf9 146 init_flag = 1;
bbw 6:a9cc2c424cf9 147 }
bbw 6:a9cc2c424cf9 148 }
bbw 6:a9cc2c424cf9 149 }
bbw 6:a9cc2c424cf9 150
bbw 6:a9cc2c424cf9 151
bbw 6:a9cc2c424cf9 152
hankzhang 2:f48b0967b6cc 153 DigitalOut led1(PC_13);
techneo 0:217105958c2d 154
hankzhang 3:30d61fa10b98 155 //Serial pc(PB_10,PB_11);
hankzhang 3:30d61fa10b98 156
hankzhang 3:30d61fa10b98 157 UARTSerial *_serial;
bbw 1:0fe432e5dfc4 158
hankzhang 3:30d61fa10b98 159 UARTSerial debug_uart(PB_10, PB_11, 115200);
hankzhang 3:30d61fa10b98 160 UARTSerial wifi_uart(PA_2, PA_3, 115200);
hankzhang 3:30d61fa10b98 161
hankzhang 4:864bb8bde75c 162 //Serial wifi_uart(PA_2, PA_3, 115200);
hankzhang 4:864bb8bde75c 163 //Serial debug_uart(PB_10, PB_11, 115200);
hankzhang 4:864bb8bde75c 164
hankzhang 4:864bb8bde75c 165 char rxBuf[32];
hankzhang 4:864bb8bde75c 166 char wifi_rxBuf[32]; //receive msg from xiaomi cloud
hankzhang 4:864bb8bde75c 167 short g_isCloud = 0; //flag for connected xiaomi cloud
hankzhang 4:864bb8bde75c 168
hankzhang 4:864bb8bde75c 169 //char set_property[] = {"down set_properties "}
hankzhang 4:864bb8bde75c 170 //char get_property[] = {"down get_properties "}
hankzhang 3:30d61fa10b98 171 int i = 0;
hankzhang 3:30d61fa10b98 172
hankzhang 3:30d61fa10b98 173 ATCmdParser *_parser;
hankzhang 4:864bb8bde75c 174
hankzhang 4:864bb8bde75c 175 void led1_thread() {
hankzhang 4:864bb8bde75c 176 int length;
hankzhang 4:864bb8bde75c 177 char len[20];
hankzhang 4:864bb8bde75c 178 char l;
hankzhang 4:864bb8bde75c 179 int position = 0;
hankzhang 4:864bb8bde75c 180 int error = 0;
hankzhang 4:864bb8bde75c 181
hankzhang 4:864bb8bde75c 182 while (true)
hankzhang 4:864bb8bde75c 183 {
hankzhang 4:864bb8bde75c 184 wifi_uart.write("get_down\r", 9);
hankzhang 4:864bb8bde75c 185 if(wifi_uart.readable())
hankzhang 4:864bb8bde75c 186 {
hankzhang 4:864bb8bde75c 187 length = wifi_uart.read(wifi_rxBuf, sizeof(wifi_rxBuf));
hankzhang 4:864bb8bde75c 188 if(!(strncmp(wifi_rxBuf,"down none",9)))
hankzhang 4:864bb8bde75c 189 {
hankzhang 4:864bb8bde75c 190 //if return "down none"
hankzhang 4:864bb8bde75c 191 debug_uart.write("--- none",8);
hankzhang 4:864bb8bde75c 192 }
hankzhang 4:864bb8bde75c 193 else if(!(strncmp(wifi_rxBuf,"down set_properties ",20)))
hankzhang 4:864bb8bde75c 194 {
hankzhang 4:864bb8bde75c 195 //if return "down set_properties"
hankzhang 4:864bb8bde75c 196 debug_uart.write("--- set:", 8);
hankzhang 4:864bb8bde75c 197 debug_uart.write(wifi_rxBuf, length);
hankzhang 4:864bb8bde75c 198
hankzhang 4:864bb8bde75c 199 //set properties
hankzhang 4:864bb8bde75c 200 if(wifi_rxBuf[22] == '7')
hankzhang 4:864bb8bde75c 201 {
hankzhang 4:864bb8bde75c 202 //set target-position
hankzhang 4:864bb8bde75c 203 position = atoi(&wifi_rxBuf[24]);
hankzhang 4:864bb8bde75c 204 sprintf(len, "position:%d", position);
hankzhang 4:864bb8bde75c 205 debug_uart.write(len, sizeof(len));
hankzhang 4:864bb8bde75c 206 }
hankzhang 4:864bb8bde75c 207
hankzhang 4:864bb8bde75c 208 //report result to cloud
hankzhang 4:864bb8bde75c 209 wifi_uart.write("result 2 7 0\r", 13);
hankzhang 4:864bb8bde75c 210 if(wifi_uart.readable())
hankzhang 4:864bb8bde75c 211 {
hankzhang 4:864bb8bde75c 212 length = wifi_uart.read(wifi_rxBuf, sizeof(wifi_rxBuf));
hankzhang 4:864bb8bde75c 213 debug_uart.write(wifi_rxBuf, length);
hankzhang 4:864bb8bde75c 214 }
hankzhang 4:864bb8bde75c 215 }
hankzhang 4:864bb8bde75c 216 else if(!(strncmp(wifi_rxBuf,"down get_properties ",20)))
hankzhang 4:864bb8bde75c 217 {
hankzhang 4:864bb8bde75c 218 //if return "down get_properties"
hankzhang 4:864bb8bde75c 219 debug_uart.write("--- get:", 8);
hankzhang 4:864bb8bde75c 220 debug_uart.write(wifi_rxBuf, length);
hankzhang 4:864bb8bde75c 221
hankzhang 4:864bb8bde75c 222 //report result to cloud
hankzhang 4:864bb8bde75c 223 }
hankzhang 4:864bb8bde75c 224 else if(!(strncmp(wifi_rxBuf,"down MIIO_net_change ",21)))
hankzhang 4:864bb8bde75c 225 {
hankzhang 4:864bb8bde75c 226 //if return "down MIIO_net_change"
hankzhang 4:864bb8bde75c 227 debug_uart.write(wifi_rxBuf, length);
hankzhang 4:864bb8bde75c 228 debug_uart.write("--- net:",8);
hankzhang 4:864bb8bde75c 229 if((!strncmp(&wifi_rxBuf[21], "offline", 7)))
hankzhang 4:864bb8bde75c 230 {
hankzhang 4:864bb8bde75c 231 //连接中
hankzhang 4:864bb8bde75c 232 debug_uart.write("offline\r", 8);
hankzhang 4:864bb8bde75c 233 }
hankzhang 4:864bb8bde75c 234 else if((!strncmp(&wifi_rxBuf[21], "local", 5)))
hankzhang 4:864bb8bde75c 235 {
hankzhang 4:864bb8bde75c 236 //连上路由器但未连上服务器
hankzhang 4:864bb8bde75c 237 debug_uart.write("local\r", 6);
hankzhang 4:864bb8bde75c 238 }
hankzhang 4:864bb8bde75c 239 else if((!strncmp(&wifi_rxBuf[21], "cloud", 5)))
hankzhang 4:864bb8bde75c 240 {
hankzhang 4:864bb8bde75c 241 //连上小米云服务器
hankzhang 4:864bb8bde75c 242 debug_uart.write("cloud\r", 6);
hankzhang 4:864bb8bde75c 243 g_isCloud = 1;
hankzhang 4:864bb8bde75c 244 }
hankzhang 4:864bb8bde75c 245 }
hankzhang 4:864bb8bde75c 246 else
hankzhang 4:864bb8bde75c 247 {
hankzhang 4:864bb8bde75c 248 debug_uart.write(wifi_rxBuf, length);
hankzhang 4:864bb8bde75c 249 }
hankzhang 4:864bb8bde75c 250
hankzhang 4:864bb8bde75c 251 }
hankzhang 4:864bb8bde75c 252 thread_sleep_for(400);
techneo 0:217105958c2d 253 }
techneo 0:217105958c2d 254 }
techneo 0:217105958c2d 255
bbw 1:0fe432e5dfc4 256
hankzhang 2:f48b0967b6cc 257 void led0_thread() {
hankzhang 3:30d61fa10b98 258 int length;
hankzhang 3:30d61fa10b98 259 while (1) {
hankzhang 3:30d61fa10b98 260 if(debug_uart.readable())
hankzhang 3:30d61fa10b98 261 {
hankzhang 3:30d61fa10b98 262 length = debug_uart.read(rxBuf, sizeof(rxBuf));
hankzhang 3:30d61fa10b98 263 debug_uart.write(rxBuf, length);
hankzhang 3:30d61fa10b98 264 wifi_uart.write(rxBuf, length);
hankzhang 3:30d61fa10b98 265 debug_uart.write("111\r",4);
hankzhang 3:30d61fa10b98 266 }
hankzhang 3:30d61fa10b98 267 if(wifi_uart.readable())
hankzhang 3:30d61fa10b98 268 {
hankzhang 3:30d61fa10b98 269 length = wifi_uart.read(rxBuf, sizeof(rxBuf));
hankzhang 3:30d61fa10b98 270 debug_uart.write(rxBuf, length);
hankzhang 3:30d61fa10b98 271 debug_uart.write("222\r",4);
hankzhang 3:30d61fa10b98 272
hankzhang 3:30d61fa10b98 273 }
hankzhang 3:30d61fa10b98 274 wait(0.5);
bbw 1:0fe432e5dfc4 275 }
bbw 1:0fe432e5dfc4 276 }
hankzhang 3:30d61fa10b98 277
hankzhang 3:30d61fa10b98 278
hankzhang 2:f48b0967b6cc 279 int main() {
hankzhang 3:30d61fa10b98 280 int length;
hankzhang 3:30d61fa10b98 281 led1 = 1;
hankzhang 3:30d61fa10b98 282
hankzhang 4:864bb8bde75c 283 wait(1);
hankzhang 3:30d61fa10b98 284
hankzhang 3:30d61fa10b98 285 debug_uart.write("hello world",11);
hankzhang 2:f48b0967b6cc 286
hankzhang 4:864bb8bde75c 287 WIFI_PWREN = 1;
hankzhang 3:30d61fa10b98 288
hankzhang 4:864bb8bde75c 289 //Thread thread0(osPriorityNormal, 512, nullptr, nullptr);
hankzhang 4:864bb8bde75c 290 Thread thread1(osPriorityNormal, 512, nullptr, nullptr);
hankzhang 3:30d61fa10b98 291 //Thread thread2(osPriorityNormal, 512, nullptr, nullptr);
hankzhang 2:f48b0967b6cc 292
hankzhang 4:864bb8bde75c 293 //thread0.start(led0_thread);
hankzhang 4:864bb8bde75c 294 thread1.start(led1_thread);
hankzhang 2:f48b0967b6cc 295
hankzhang 3:30d61fa10b98 296 //wifi_serial.printf("model\r\n");
hankzhang 3:30d61fa10b98 297 //thread1.start(led1_thread);
hankzhang 3:30d61fa10b98 298 //thread2.start(led2_thread);
hankzhang 3:30d61fa10b98 299 while(1)
hankzhang 3:30d61fa10b98 300 {
hankzhang 3:30d61fa10b98 301 debug_uart.write("--main--\r\n",10);
hankzhang 4:864bb8bde75c 302 if(g_isCloud)
hankzhang 4:864bb8bde75c 303 {
hankzhang 4:864bb8bde75c 304 debug_uart.write("connect to xiaomi cloud\r\n",25);
hankzhang 4:864bb8bde75c 305 }
hankzhang 3:30d61fa10b98 306 wait(3);
bbw 1:0fe432e5dfc4 307 }
bbw 6:a9cc2c424cf9 308
bbw 6:a9cc2c424cf9 309
bbw 6:a9cc2c424cf9 310 system_init();
bbw 6:a9cc2c424cf9 311 InterruptIn Hall1(PA_14);
bbw 6:a9cc2c424cf9 312 //InterruptIn Hall2(PB_3);
bbw 6:a9cc2c424cf9 313 Hall1.fall(callback(sensor_capture_cb)); // Attach ISR to handle button press event
bbw 6:a9cc2c424cf9 314 //Hall2.fall(callback(sensor_capture_cb)); // Attach ISR to handle button press event
bbw 6:a9cc2c424cf9 315 debug_uart.printf("Hall sensor init done\r\n");
hankzhang 3:30d61fa10b98 316
bbw 6:a9cc2c424cf9 317 Thread thread1(Power_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE); /*check the real-time current*/
bbw 6:a9cc2c424cf9 318 //debug_uart.printf("thread1~~~~~~~~~~~~~~~~\r\n");
bbw 6:a9cc2c424cf9 319 Thread thread2(Motor1_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE); /*check the real-time current*/
bbw 6:a9cc2c424cf9 320 //debug_uart.printf("thread2~~~~~~~~~~~~~~~~\r\n");
bbw 6:a9cc2c424cf9 321 Thread thread3(Motor2_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE); /*check the real-time current*/
bbw 6:a9cc2c424cf9 322 //debug_uart.printf("thread3~~~~~~~~~~~~~~~~\r\n");
bbw 6:a9cc2c424cf9 323
bbw 6:a9cc2c424cf9 324 debug_uart.printf("three threads created\r\n");
bbw 6:a9cc2c424cf9 325
bbw 6:a9cc2c424cf9 326 while(1){
bbw 6:a9cc2c424cf9 327 //debug_uart.printf("~~~~~~~~~~~~~~\r\n");
bbw 6:a9cc2c424cf9 328 if(ready_flag){
bbw 6:a9cc2c424cf9 329 m_val += sense_value;
bbw 6:a9cc2c424cf9 330 if(sense_value>0.5){
bbw 6:a9cc2c424cf9 331 motor2_move(STOP);
bbw 6:a9cc2c424cf9 332 }
bbw 6:a9cc2c424cf9 333 }
bbw 6:a9cc2c424cf9 334 //sense_value = SensorCurrent.read();
bbw 6:a9cc2c424cf9 335 //if(sense_value > 0.7){
bbw 6:a9cc2c424cf9 336 //debug_uart.printf("Main_thread: WARNING WARNING WARNING current data is higher than 0.7\r\n");
bbw 6:a9cc2c424cf9 337 //}
bbw 6:a9cc2c424cf9 338 }
bbw 6:a9cc2c424cf9 339
bbw 6:a9cc2c424cf9 340 }
bbw 6:a9cc2c424cf9 341
bbw 6:a9cc2c424cf9 342 void motor1_move(uint8_t dir){/*main motor*/
bbw 6:a9cc2c424cf9 343 if(dir==1){/*forward*/
bbw 6:a9cc2c424cf9 344 MOTOA1 = 0;
bbw 6:a9cc2c424cf9 345 MOTOB1 = 1;
bbw 6:a9cc2c424cf9 346 }else if(dir==2){/*backward*/
bbw 6:a9cc2c424cf9 347 MOTOA1 = 1;
bbw 6:a9cc2c424cf9 348 MOTOB1 = 0;
bbw 6:a9cc2c424cf9 349 }else{ /*stop*/
bbw 6:a9cc2c424cf9 350 MOTOA1 = 0;
bbw 6:a9cc2c424cf9 351 MOTOB1 = 0;
bbw 6:a9cc2c424cf9 352 }
bbw 6:a9cc2c424cf9 353 }
bbw 6:a9cc2c424cf9 354
bbw 6:a9cc2c424cf9 355 void motor2_move(uint8_t dir){/*assistant motor*/
bbw 6:a9cc2c424cf9 356 if(dir==1){/*up*/
bbw 6:a9cc2c424cf9 357 MOTOA2 = 0;
bbw 6:a9cc2c424cf9 358 MOTOB2 = 1;
bbw 6:a9cc2c424cf9 359 }else if(dir==2){/*down*/
bbw 6:a9cc2c424cf9 360 MOTOA2 = 1;
bbw 6:a9cc2c424cf9 361 MOTOB2 = 0;
bbw 6:a9cc2c424cf9 362 }else{ /*stop*/
bbw 6:a9cc2c424cf9 363 MOTOA2 = 0;
bbw 6:a9cc2c424cf9 364 MOTOB2 = 0;
bbw 6:a9cc2c424cf9 365 }
bbw 6:a9cc2c424cf9 366 }
bbw 6:a9cc2c424cf9 367
bbw 6:a9cc2c424cf9 368 void system_init(){
bbw 6:a9cc2c424cf9 369 debug_uart.baud(115200);
bbw 6:a9cc2c424cf9 370 MOTOA1 = 0;
bbw 6:a9cc2c424cf9 371 MOTOB1 = 0;
bbw 6:a9cc2c424cf9 372 MOTOA2 = 0;
bbw 6:a9cc2c424cf9 373 MOTOB2 = 0;
bbw 6:a9cc2c424cf9 374 init_flag = 0;
bbw 6:a9cc2c424cf9 375 motor1_ready_flag = 0;
bbw 6:a9cc2c424cf9 376 motor2_ready_flag = 0;
bbw 6:a9cc2c424cf9 377 sense_value = 0;
bbw 6:a9cc2c424cf9 378 sensor_flag = 0;
bbw 6:a9cc2c424cf9 379 ready_flag = 0;
bbw 6:a9cc2c424cf9 380 debug_uart.printf("*******************************\r\n");
bbw 6:a9cc2c424cf9 381 debug_uart.printf("**********LAIWU TECH***********\r\n");
bbw 6:a9cc2c424cf9 382 debug_uart.printf("*******************************\r\n");
bbw 6:a9cc2c424cf9 383 debug_uart.printf("system init done, wait 3 seconds to start\r\n");
bbw 6:a9cc2c424cf9 384 wait(3);
hankzhang 2:f48b0967b6cc 385 }