Funded by National Science Foundation (NSF)
Instructor: Dr. Yier Jin, 352-294-0401, yier.jin@ece.ufl.edu
Student Assistants: Yubin Liu, Ziyang Liu, Max Panoff
Building an autopilot vehicle fleet by combining the TI-RSLK module with sensor such as ultrasonic and camera.
Autonomous Vehicles (AV) are a fast growing field as supporting technologies, such as machine learning, computer vision, and advanced sensors are becoming increasing refined. Interestingly, teams or networks of AVs, known as fleets, are gaining less notoriety than individual AVs, despite the many potential advantages in efficiency and speed fleets hold over their solitary peers.
We have designed and made a simple model to demonstrate the basic function of autopilot vehicle fleet (Fig.1). The vehicle fleet contains three vehicles as an example. We use the components mainly offered by TI-RSLK and some other electronic devices such as Raspberry Pi, cameras, Wi-Fi modules and ultrasonic sensors. The information flow of the vehicle fleet is depicted in Fig.2. The first vehicle has three ultrasonic sensors in different directions to detect and avoid obstacles automatically. It also can send commands directly to the other two vehicles by Wi-Fi modules as a server so that they can do the same actions simultaneously. This can realize in either remote or local network. It is worth noticing that only the first vehicle has ultrasonic sensors because we hope these three vehicles perform as a formation instead of driving separately, which means that only the first vehicle has the mission to detect obstacles. To solve the deviation problem, we use a camera attached to Raspberry Pi 3B+ to recognize the symbol mounted on the back of the vehicle, which means that the second and the third vehicle can follow the vehicle in front of it to keep the vehicle fleet in alignment.
The framework of each vehicle is shown in Fig.3. We use MSP432P401R as the main processor which process the main thread of the whole system. It can run C++ project conveniently. The MSP432 Launchpad Development Kit offered by TI is really good to use. Most of the vehicles’ components are offered by TI Robotics System Learning Kit (TI-RSLK). The official website for these kits is: https://university.ti.com/en/faculty/ti-robotics-system-learning-kit/ti-robotics-system-learning-kit. TI also offers a comprehensive curriculum for it. The official website of the curriculum is: https://university.ti.com/en/faculty/ti-robotics-system-learning-kit/ti-robotics-system-learning-kit/curriculum-design-launch. It has many modules which offer some key information to start the explore. As we plan to develop a model which is based on the function we suppose, we only reference part of the curriculum instead of finishing all the modules of the curriculum in order as the official recommend. We use CC3120BOOST as the Wi-Fi module to keep communication between the vehicles. The CC3120 module uses MQTT communication standard to connect with cloud brokers or local clients, so that it can send the command from the smartphones or servers to MSP432 as a bridge. The basic codes have been offered by TI and we need to modify some parts to apply them in this system. We also use the mobile phone as a local client to send and receive some commands and get linked with a cloud broker. To keep all the vehicles in alignment, we use Raspberry Pi and camera to develop a real-time correction system. The Raspberry Pi can run python files easily and can be controlled visually by VNC Viewer on a laptop. The Raspberry Pi processes the picture data captured by the camera and sends the commands to MSP432 using serial communication. The MSP432 received the location information of the front vehicle from Pi or the commands from CC3120. Then it controls the output electrical levels in order to make the motors do timely response. In the following article, we will discuss the key process and method we have experienced and give the problems and suggests about the project in detail.
We adopt Code Composer Studio (CCS) as the IDE to use. It can be downloaded from: http://software-dl.ti.com/ccs/esd/documents/ccs_downloads.html. Our release is CCS 9.1.0 for a 64bit Windows System. The IDE is developed for TI microcontrollers and embedded processors and is very powerful. What’s more, the official website also offers some useful reference documents which are downloaded in the Documents folder. We also need to download a software called TI-RSLK Maze which offers many example projects and unfinished code framework. The website to download is: http://www.ti.com/general/docs/lit/getliterature.tsp?baseLiteratureNumber=SLAC768&fileType=zip.
We first create a new workspace. Then we import all the C++ project folders into CCS. When selecting the file type of folders to import, select CCS project rather than C/C++. After that, we can find these C++ projects in the Project Explorer of CCS, which means we can start the exploration now. If we click Debug, the debug operation leads to several actions done automatically. It will prompt to save source files, build the project (incrementally), start the debugger, connect CCS to the target, load the program on the target and run to main.
Almost all the steps of hardware construction are offered by the curriculum mentioned before. We reference mostly the Module 5 and Module12. There are three main components we need to build connection in the process of realizing basic function: Motor Drive and Power Distribution Board (MDPDB), MSP432 LaunchPad (Fig.4) and motors.
MDPDB is a black board to offer the regulated +5V power for the vehicle. The next two figures from the slides for Lab 5 in the tutorial provided by TI (Fig.5and Fig.6) are the key procedures to connect the MDPDB and LaunchPad. There will not be serious problem following the procedures on the two pages.
The pins of MDPDB are shown in Fig.7. For the motors, each of them has two pins, we just need to connect them to the ML+/ML- and MR+/MR- on the MDPDB and weld the jumpers on the two pins to fix them. To add the Wi-Fi module in future, we use P7.x, P8.x and P9.x to control the motors instead of P1.x, P2.x and P3.x. And they need to connect with these pins on the MDPDB.
After doing these steps, we have finished the basic construction of the hardware, then we should write codes to make the vehicle drive. We will introduce each important part of the vehicle in detail in the following article.
We start by coding the DC Motors. Several C files are needed to run the vehicle. The most important file in DC motors module is MotorSimple.c. It is in the inc folder so that it can be included in other projects conveniently. There are several functions in this C file. From Module 6 - GPIO, we learn how to initialize and use GPIO as we want.
Motor_InitSimple: This function is to initialize the 6 GPIO pins (P7.6, P7.7, P8.6, P8.7, P9.6 and P9.7). Module 6 gives instruction about the configuration.
void Motor_InitSimple(void){ P7->SEL0 &= ~0xC0; P7->SEL1 &= ~0xC0; P7->DIR |= 0xC0; P8->SEL0 &= ~0xC0; P8->SEL1 &= ~0xC0; P8->DIR |= 0xC0; P9->SEL0 &= ~0xC0; P9->SEL1 &= ~0xC0; P9->DIR |= 0xC0; }
Motor_StopSimple: This function is to stop both motors.
void Motor_StopSimple(void){ P7->OUT &= ~0xC0; P8->OUT &= ~0xC0; P9->OUT &= ~0xC0; }
The following functions enable the vehicles to run in different directions. The common principle is to control the different rotating speed between left motor and right motor. Thus we will only introduce the forward function in detail and other functions are almost the same with the forward function.
Motor_ForwardSimple: There are three formal parameters, duty, period and time, in this function. The first two parameters decide the duty cycle, which means that they can determine the rotating speed. There is a for loop which occupies the function. As the PWM pins on MDPDB connect with P8.6 and P8.7, we should use these two pins and SysTick function to make a suitable duty cycle. Px.6 and Px.7 is 0xx0000 in binary, so 0xC0 is the hexadecimal format. SysTick function use the 48MHz clock to control the time as user want. SysTick_Wait10ms means the time unit of keeping the last operation is 10ms. User can call Motor_ForwardSimple by Motor_ForwardSimple(1,2,300), which means the vehicle will forward for 3s by a 50 percent duty cycle. After the operation, we need to stop moving like Motor_StopSimple function.
void Motor_ForwardSimple(uint16_t duty, uint16_t period, uint32_t time){ uint16_t i=0; for(i=0;i<time;i=i+period){ P7->OUT &= ~0xC0; P8->OUT |= 0xC0; SysTick_Wait10ms(duty); P8->OUT &= ~0xC0; SysTick_Wait10ms(period-duty); P9->OUT |= 0xC0; } P7->OUT &= ~0xC0; P8->OUT &= ~0xC0; P9->OUT |= 0xC0; }
Motor_BackwardSimple:
void Motor_BackwardSimple(uint16_t duty, uint16_t period, uint32_t time){ uint16_t i=0; for(i=0;i<time;i=i+period){ P7->OUT |= 0xC0; P8->OUT |= 0xC0; SysTick_Wait10ms(duty); P8->OUT &= ~0xC0; SysTick_Wait10ms(period-duty); P9->OUT |= 0xC0; } P7->OUT &= ~0xC0; P8->OUT &= ~0xC0; P9->OUT |= 0xC0; }
Motor_LeftSimple
void Motor_LeftSimple(uint16_t duty, uint16_t period, uint32_t time){ uint16_t i=0; for(i=0;i<time;i=i+period){ P7->OUT &= ~0xC0; P8->OUT |= 0x40; SysTick_Wait10ms(duty); P8->OUT &= ~0x40; SysTick_Wait10ms(period-duty); P9->OUT |= 0xC0; } P7->OUT &= ~0xC0; P8->OUT &= ~0xC0; P9->OUT |= 0xC0; }
Motor_LeftCircleSimple
void Motor_LeftCircleSimple(uint16_t duty, uint16_t period, uint32_t time){ uint16_t i=0; for(i=0;i<time;i=i+period){ P7->OUT |= 0x80; P8->OUT |= 0xC0; SysTick_Wait10ms(duty); P8->OUT &= ~0xC0; SysTick_Wait10ms(period-duty); P9->OUT |= 0xC0; } P7->OUT &= ~0xC0; P8->OUT &= ~0xC0; P9->OUT |= 0xC0; }
Motor_RightSimple
void Motor_RightSimple(uint16_t duty, uint16_t period, uint32_t time){ uint16_t i=0; for(i=0;i<time;i=i+period){ P7->OUT &= ~0xC0; P8->OUT |= 0x80; SysTick_Wait10ms(duty); P8->OUT &= ~0x80; SysTick_Wait10ms(period-duty); P9->OUT |= 0xC0; } P7->OUT &= ~0xC0; P8->OUT &= ~0xC0; P9->OUT |= 0xC0; }
Motor_RightCircleSimple
void Motor_RightCircleSimple(uint16_t duty, uint16_t period, uint32_t time){ uint16_t i=0; for(i=0;i<time;i=i+period){ P7->OUT |= 0x40; P8->OUT |= 0xC0; SysTick_Wait10ms(duty); P8->OUT &= ~0xC0; SysTick_Wait10ms(period-duty); P9->OUT |= 0xC0; } P7->OUT &= ~0xC0; P8->OUT &= ~0xC0; P9->OUT |= 0xC0; }
The difference between Motor_LeftSimple and Motor_LeftCircleSimple is small. The left motor will rotate to forward but the right motor will not rotate at all when using Motor_LeftSimple. However, the left motor will still rotate to forward but the right motor will rotate to backward when using Motor_LeftCircleSimple. The difference between Motor_RightSimple and Motor_RightCircleSimple is similar with this. Obviously, we use mainly Motor_LeftCircleSimple and Motor_RightCircleSimple, because that makes the vehicle change direction more efficiently. Another thing we need to notice is that as we change the numbers of the formal parameters, we also need to modify the head file of this project. We need to add another formal parameter in every function declaration and add two other functions as we mentioned before. And we can write a simple main function in Lab12_Motorsmain.c to test our codes. Remember to initialize all the devices before to call their functions.
int main(void){ Clock_Init48MHz(); LaunchPad_Init(); Bump_Init(); SysTick_Init(); Motor_InitSimple(); LaunchPad_Output(0x02); Motor_ForwardSimple(1,2,300); Motor_BackwardSimple(1,2,300); Motor_LeftSimple(1,2,100); Motor_RightSimple(1,2,100); Motor_LeftCircleSimple(1,2,100); Motor_RightCircleSimple(1,2,100); }
If all the files are built correctly, when you run the vehicle, the vehicle will forward for 3s, backward for 3s, keep turning left for 1s, keep turning right for 1s, keep turning left in circle for 1s and keep turning right in circle for 1s in order.
From this module, we hardly reference the curriculum as before because the functions we want to realize are based on our design. The Wi-Fi module we use is TI’s SimpleLink Wi-Fi CC3120 (Fig.8). We use a project named network_terminal_MSP_EXP432P401R-tirtos_ccs to learn how to use the Wi-Fi module. The project is a basic project which allows the user to experience the basic function of Wi-Fi module. When we set up successfully, we can open a terminal to look over the commands we can use as shown in Fig.9 and Fig.10.
The most important projects in this experiment are mqtt_client_MSP_EXP432P401R_tirtos_ccs and mqtt_client_server_MSP_EXP432P401R_tirtos_ccs. These two projects are the application of the Message Queuing Telemetry Transport (MQTT). MQTT is a machine to machine (M2M) and Internet of Things (IoT) connectivity protocol. It was designed as an extremely lightweight publish/subscribe messaging transport. It is useful for connections with remote locations where a small code footprint is required and network bandwidth is at a premium. It has been used in sensors communicating to a broker via satellite link, over occasional dial-up connections with healthcare providers, and in a range of home automation and small device scenarios. It is also ideal for mobile applications because of its small size, low power usage, minimized data packets, and efficient distribution of information to one or many receivers.
In our model, the first vehicle is set as a server, the second and the third vehicle are set as clients. We have many methods to control them. Now we will introduce two modes we ever used. One way is that we send commands from a mobile phone to the server, then the server will send the same commands to other clients. The other way is that we make the server drive automatically (it can detect and avoid the obstacles by three ultrasonic sensors), then the other clients drive as the server.
To begin with, we need to modify some codes to connect to a Wi-Fi access point. To run the system more conveniently, we use a mobile phone as a mobile hotspot. We need to change the SSID_NAME, SECURITY_TYPE and SECURITY_KEY to ours, in the head file named network_if.h (Fig.11). Both the client and server projects need to operate like this. Now we use the server project as an example. Both the client and server projects need to operate like this. Now we use the server project as an example.
We need to get the IP address of the server to allow other clients to connect with it. We can use any project about Wi-Fi mentioned before to get the IP address of any vehicle. Besides, we still need a remote broker to help us connect with a remote MQTT client. As we can see from Fig.12, the IP address of the server is “192.168.43.64” and the remote broker is “mqtt.eclipse.org”. As MQTT is based on publish and subscribe, SUBSCRIPTION_TOPIC and PUBLISH_TOPIC are very important. We can see from Fig.13 that the server’s PUBLISH_TOPIC is “/cc32xx/To/Broker”, ”, it’s SUBSCRIPTION_TOPIC is “/Broker/To/cc32xx”, which means the server can publish the commands by “/cc32xx/To/Broker” topic, the clients which has subscribed “/cc32xx/To/Broker” topic can receive the commands from the server and the server also can receive the messages from the “/Broker/To/cc32xx” topic. Notice that the publish and subscription topic can be the same. As the server’s PUBLISH_TOPIC is “/cc32xx/To/Broker” and we want to make all the clients receive the server’s commands, we need to change the clients’ SUBSCRIPTION_TOPIC to “/cc32xx/To/Broker”. We don’t need to change the clients’ PUBLISH_TOPIC because the clients don’t need to publish any messages in the experiment. All above parameters can be found in the mqtt_server_app.c and mqtt_client_app.c.
Then we need to download an App called MyMQTT. In this experiment, the mobile phone can be set as either local client or remote client, this can be determined by whether the mobile phone is connected to the same WiFi AP. When we connect the mobile phone with “mqtt.eclipse.org” (Fig.14) (The Username and Password can be set in the mqtt_server_app.c (Fig.15).), the mobile phone as a client (both local or remote client can be realized) publish the message to a remote broker, and the remote broker will publish the same message to those clients which subscribe the same topic. When we connect the mobile phone with “192.168.43.64” (the IP address of the vehicle server), the mobile phone as a local client publish the message to a local MQTT server (local broker and cloud client), the local server will publish the same message to those local clients which subscribe the same topic. We can also subscribe some topic on the mobile phone by the APP, which means we can also receive many messages between the server and the clients on the phone.
The next step is to make the vehicles drive as directed through the app. To begin with, we need to write a function to realize the control to the vehicles. We design a function called VehicleMove, it has a formal parameter which presents the command we want to send. The basic command is “forward”, “backward”, “left” and “right”. Remember to initialize the functions before using them. What’s more, the speed of the vehicle can be changed by the functions mentioned before.
void VehicleMove (char *string){ Clock_Init48MHz(); LaunchPad_Init(); Motor_InitSimple(); SysTick_Init(); char Forward[8]="forward"; char Backward[9]="backward"; char Left[5]="left"; char Right[6]="right"; int flag=0; if (strcmp(string,Forward)==0) flag=1; else if (strcmp(string,Backward)==0) flag=2; else if (strcmp(string,Left)==0) flag=3; else if (strcmp(string,Right)==0) flag=4; switch (flag) { case 1: LaunchPad_Output(0x02); Motor_ForwardSimple(1,3,30); Systick_Wait10ms(10); break; case 2: LaunchPad_Output(0x01); Motor_BackwardSimple(1,3,30); Systick_Wait10ms(10); break; case 3: LaunchPad_Output(0x03); Motor_LeftCircleSimple(1,3,30); Systick_Wait10ms(10); break; case 4: LaunchPad_Output(0x03); Motor_RightCircleSimple(1,3,30); Systick_Wait10ms(10); break; default: LaunchPad_Output(0x00); Motor_StopSimple(); } }
The VehicleMove function for the clients is a little different from the server’s VehicleMove function. The VehicleMove function for the clients has another “keep” command, the command can be sent from the Raspberry Pi Which we will introduce in the following article. An important thing we need to notice is that we need to include all the related head files and copy all these c files to the project before we debug and build the whole project (Fig.16). If we could not operate like this, there would be many errors and warnings.
To call the VehicleMove function, we still need to change some other codes. We define the VehicleMove function function in the server_client_cbs.c file, because we need to use the commands from a mobile phone as a client. To obtain the commands, we define a variable named ‘t’, which is used for extracting the information from the mobile phone.
t=pubBuff + payloadOffset;
It needs to be placed in MQTTServer_RECV_CB_EVENT, MqttServerCallback function, server_client_cbs.c file. As ‘t’ will be called in mqtt_server_app.c file, we need to set ‘t’ as extern char type in server_client_cbs.c file.
extern char *t;
We need to define ‘t’ as char type in mqtt_server_app.c to call VehicleMove(t).
char *t;
The place where VehicleMove function is called determines the difference between the first mode and the second mode in server project. In the first mode, the vehicle server needs to wait for the signals sent from the mobile phone as a client, so we need to insert the VehicleMove(t) in an infinite for loop in mqtt_server_app.c. We can insert VehicleMove(t) at the end of both case MSG_RECV_BY_SERVER and case MSG_RECV_BY_CLIENT to execute commands sent from both the local server and remote broker. The reason why we need add the function at the end of these two cases is that the vehicle server needs to send out the commands before it executes these commands to reduce the time delay of clients’ executing.
In the client project, most of the operations are like the server project, but the VehicleMove(t) function needs to be inserted just before the switch in the same infinite for loop. Following is the process to run the first mode:
Although the vehicle clients will take the same actions as the vehicle server, the result of this mode is not very well, because we need to input every command to these vehicles, which means that these vehicles are not intelligent. What’s more, although these vehicles will execute the same commands, they still may not drive as we want in a straight line because the ground is not horizontal enough and the wheels are not circle enough. So we need to explore another mode to make these vehicles more intelligent and correct the deviation in a suitable and simple method.
There is no difference between the client projects in the first mode and the second mode, because the clients in these two modes execute the commands from the vehicle server in the same way. However, the difference between the server projects in these two modes are very huge. To begin with, we need to delete the VehicleMove(t) function in the server project. Then, in order to make the vehicle server more intelligent, which means the vehicle server can autopilot, we add an ultrasonic module on the vehicle server. The ultrasonic module will be introduced in the following article. We just talk about the effect of the ultrasonic module and the principle of the mode here.
As we need to make the vehicle server drive constantly. At the same time, the vehicle server needs to send the same commands to other vehicle clients. This process will continue until we let them stop. So we need to change the mainThread function in the mqtt_server_app.c file. In the infinite while loop of the mainThread function, we need to find another while loop.
while(gResetApplication == false) { //sleep(1); // add by myself Clock_Init48MHz(); LaunchPad_Init(); // built-in switches and LEDs int left,middle,right; char Forward[8]="forward"; char Backward[9]="backward"; char Left[5]="left"; char Right[6]="right"; char Topic[]="/cc32xx/To/Broker"; struct publishMsgHeader msgHead; uint32_t flags; flags = msgHead.qos; if(msgHead.retain) { flags = flags | MQTT_PUBLISH_RETAIN; } flags=2; Echo_init(); Motor_InitSimple(); // initialization while(1){ LaunchPad_Output(0x02); middle=Echo_loop_middle(); left=Echo_loop_left(); right=Echo_loop_right(); if (middle<=200||right<=200||left<=200){ MQTTServer_publish(gMqttClient, (char *) Topic,strlen((char *) Topic), (char*) Backward, strlen((char*) Backward),flags); Motor_BackwardSimple(1,2,50); //SysTick_Wait10ms(10); if (right>=left && middle>200){ MQTTServer_publish(gMqttClient, (char *) Topic,strlen((char *) Topic), (char*) Right, strlen((char*) Right),flags); Motor_RightCircleSimple(1,3,30); //SysTick_Wait10ms(10); } else if(left>right && middle>200){ MQTTServer_publish(gMqttClient, (char *) Topic,strlen((char *) Topic), (char*) Left, strlen((char*) Left),flags); Motor_LeftCircleSimple(1,3,30); //SysTick_Wait10ms(10); } } else if(middle<500 || left<500 || right<500){ if(middle>=right && middle>=left){ MQTTServer_publish(gMqttClient, (char *) Topic,strlen((char *) Topic), (char*) Forward, strlen((char*) Forward),flags); Motor_ForwardSimple(1,2,50); //SysTick_Wait10ms(10); } else if (right>middle && right>=left){ MQTTServer_publish(gMqttClient, (char *) Topic,strlen((char *) Topic), (char*) Right, strlen((char*) Right),flags); Motor_RightCircleSimple(1,3,30); //SysTick_Wait10ms(10); } else if (left>right && left>middle){ MQTTServer_publish(gMqttClient, (char *) Topic,strlen((char *) Topic), (char*) Left, strlen((char*) Left),flags); Motor_LeftCircleSimple(1,3,30); //SysTick_Wait10ms(10); } // else{ // // SysTick_Wait10ms(10); // } } else{ MQTTServer_publish(gMqttClient, (char *) Topic,strlen((char *) Topic), (char*) Forward, strlen((char*) Forward),flags); Motor_ForwardSimple(1,2,50); } }
We need to define all the variables firstly. These variables are similar as the variables in VehicleMove function because the basic function is almost same. Besides, we need to add a new publish topic with the same content because the vehicle server will not need to send and execute the commands from the mobile phone in the second mode. In the first mode, the function about sending commands have been called in case MSG_RECV_BY_SERVER and case MSG_RECV_BY_CLIENT. We will not use them at all in the second mode, so we need to realize by ourselves.
We can use the ultrasonic module (Fig.20) after initializing the module. The basic purpose of the ultrasonic module is detecting and avoiding obstacles, so we should realize the basic logic relationship according to this basic purpose.
The distance unit is dependent on the ultrasonic module which will be introduced in the following article. Following is the procedure to run the second mode:
By this algorithm, the vehicle server can hardly collide the obstacles in the lab, but sometimes it may encounter with hesitant condition so that it stay in the same place for a long time. The second and the third vehicle clients will take the same action as the vehicle server, but they do not have any sensors except Wi-Fi modules so they can only execute the commands rigidly whatever happens to them. Meanwhile, the deviation problem is still not dealt with. So the overall effect is that although these vehicles will take the same actions automatically all the time, they will seem untidy after a while.
We use HC-SR04 as our ultrasonic sensors (Fig.23). For more information, you can read the data sheet HC-SR04.pdf. Connect the VCC to 5V and the grounds on MSP432, then use the Dupont Wires to attach the Echo to P6.3/P6.4/P6.5 and the Trig to P5.3/P5.4/P5.5 as shown in Fig.2. Then you can supply a shorter pulse on Trig to get the distance sensed. The functions on MSP432 are as follows.
int Echo_init(void){ // Initializes the GPIO lines and puts sensors to wait P6->SEL0 &= ~0x38; P6->SEL1 &= ~0x38; P6->DIR &= ~0x38; //make P6.3 P6.4 P6.5 input P5->SEL0 &= ~0x38; P5->SEL1 &= ~0x38; P5->DIR |= 0x38; //make P5.3 P5.4 P5.5 output P5->OUT &= ~0x38; //make P5.3 P5.4 P5.5 output low } int Echo_loop_right(){ SysTick_Init(); P5->OUT &= ~0x08; SysTick_Wait1us(5000); P5->OUT |= 0x08; //make a pulse on Trig SysTick_Wait1us(10000); P5->OUT &= ~0x08; int flag=0; //skip invalid time on Echo while((P6->IN & 0x08) == 0){ SysTick_Wait1us(1); flag++; if (flag>=10000) //prevent endless circulation break; } int count; //get the high level time on Echo count=0; while ((P6->IN & 0x08) == 8){ SysTick_Wait1us(1); count++; if (count>=60000){ count=10000; break; } } return count; }
Echo_loop_right (), Echo_loop_middle () and Echo_loop_left () are the functions to use three sensors in different directions. We only paste one function here because they have similar structures. As for the return of the function, we did some experiments to test how to transform the time information to distance information. Finally, we use the linear fitting to find the formula of the relationship between them (Fig.24) and the formula is:
In order to solve the deviation problem, we add a Raspberry Pi and a Camera on the two vehicle clients separately. We use the supplementary camera to capture a special mark at the rear of each vehicle, use the Raspberry Pi 3B+ to calculate and send the correction information (Fig.25). The communication between the Raspberry Pi and the MSP432 LaunchPad depends on GPIO pins of these boards. As for the power supply of the Raspberry Pi, we find that it is not enough by using the power from MDPDB, so we use a mobile power bank as the battery of Raspberry Pi.
We use the camera V2 connected with Pi to detect the symbols made by ourselves. The symbols are 3D models made by 3D printer FlashForge Creator Pro. We use 3D builders to draw the models and the software downloaded from official website of the printer to print them. The models are as shown in Fig.26. You can open the document Symbol.3mf for the parameters and more details. You can let the columns go through the two hollow cylinders to fix it on the vehicles back, then it can be detected by the cameras.
There are many necessary preparation procedures before we can use the Raspberry Pi and camera to capture and process pictures. We will introduce them in detail in the following.
ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev network={ ssid="Your Wi-Fi name" psk="password" }After these operations, the Raspberry Pi will be allowed to connect to the Wi-Fi AP automatically so that we can change some files further.
sudo raspi-config
raspistill -v -o test.jpgto test the camera.
sudo apt-get install build-essential cmake git pkg-config sudo apt-get install libjpeg8-dev sudo apt-get install libtiff5-dev sudo apt-get install libjasper-dev sudo apt-get install libpng12-dev sudo apt-get install libavcodec-dev libavformat-dev libswscale-dev libv4l-dev sudo apt-get install libgtk2.0-dev sudo apt-get install libatlas-base-dev gfortran
cd opencv-3.4.6 mkdir release cd release cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr .. sudo make sudo make install sudo ldconfigIt will cost us more than two hours in the forth command so that we should ensure the connection of the Wi-Fi signal and adequate electrical power supply during this time.
lower_green=np.array([35,43,46]) upper_green=np.array([77,255,255])Then we need to add some code to create a double-ended queue. Next, we should open the camera and transfer the color model. The key procedure is to find the center of the board, we use the center of the circumscribed circle as the center of the green board. Then we need to send related commands according to the relative position of the board in the screen. The code is in the following.
from collections import deque from picamera.array import PiRGBArray from picamera import PiCamera import time import cv2 import numpy as np import math import serial ser=serial.Serial('/dev/ttyAMA0',115200,timeout=0.5) lower_green=np.array([35,43,46]) upper_green=np.array([77,255,255]) mybuffer = 64 pts = deque(maxlen=mybuffer) camera = PiCamera() camera.resolution = (640, 480) camera.framerate = 32 rawCapture = PiRGBArray(camera, size=(640, 480)) time.sleep(1) for image in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True): frame = image.array hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv, lower_green, upper_green) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] center = None if len(cnts) > 0: c = max(cnts, key = cv2.contourArea) ((x, y), radius) = cv2.minEnclosingCircle(c) M = cv2.moments(c) center = (int(M["m10"]/M["m00"]), int(M["m01"]/M["m00"])) print (center) print (int(radius)) if radius > 10: cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255), 2) cv2.circle(frame, center, 5, (0, 0, 255), -1) pts.appendleft(center) left="right\r" right="left\r" forward="forward\r" backward="backward\r" keep="keep\r" if int(radius<64): ser.write(keep.encode("ISO-8859-1")) print("keep") elif (center[0]-320)>int(radius): ser.write(left.encode("ISO-8859-1")) print ("left") elif ((center[0]-320)<(-int(radius))): ser.write(right.encode("ISO-8859-1")) print ("right") else: ser.write(keep.encode("ISO-8859-1")) print("keep") cv2.imshow("Frame", frame) rawCapture.truncate(0) if cv2.waitKey(1) & 0xFF == ord('q'): breakThis is the code we used finally. We have written and tested many other codes to test the camera and the algorithm in the beginning. We won’t present those code here anymore.
First, we need to turn on the Raspberry Pi and connects it with the laptop as before. Then we need to open the python IDE called Thonny (Fig.30). We can compile and build the code here.
After the preparation, we can run the code and find the camera work well (Fig.31).
We also need to change the program in the MSP432 LaunchPad to process the signals from the GPIO ports on the Raspberry Pi board. First, we need to delete the code in the for loop mentioned before, because we won’t receive messages by Wi-Fi module so that the for loop will stop in the beginning. Second, we need to add some codes in the infinite while loop, mainThread function, mqtt_client_app.c file.
while(gResetApplication == false) { char tt[20]; UART0_Init(); UART0_InString(tt,19); VehicleMove(tt); }
Then we can operate as the method before to turn on the vehicle and Raspberry Pi, open the VNC Viewer as we mentioned before and run the python file test2.py to test the effect of this method. Unfortunately, the effect of this method is not good, which means that it is very difficult to only use the camera as the sensor to finish the whole process. We need to slow down the speed of the vehicle server and add some waiting time to ensure the vehicle client follow the first vehicle. So it is not a good idea to just use a camera or a Wi-Fi module as the only sensor. We need to combine these two sensors to realize a good effect.
We need to change the program on the base of the second mode of the vehicle client. We only need to add the related codes about camera mentioned before in the for loop of the mqtt_client_app.c file.
for(;; ) { char tt[20]; UART0_Init(); UART0_InString(tt,19); mq_receive(g_PBQueue, (char*) &queueElemRecv, sizeof(struct msgQueue), NULL); VehicleMove(tt); VehicleMove(t); …… }
In the beginning, we ever tried to realize the effect of interrupts, which means only processing the commands from the Raspberry Pi periodically. However, the effect of correction is not very obvious, so we change to process the commands in every loop. After these operations, we need to run the whole system. It is a little inconvenient because we will use the mobile phone, laptop and the vehicles which is composed of MSP432 LaunchPad, MDPDB, CC3120 Wi-Fi module, Raspberry Pi and a mobile power bank. We can set up the whole system in this order:
The final effect is better than before, but it still can’t keep in a fleet all the time. Due to the limited time and limited cost, we can only stop the explore temporarily. We will give the advice which may be better for this system and welcome everyone to join this project.
We use UART to connect the Pi and MSP432 together for communication. Here, We’d like to introduce the function used. On MSP432, we use the default (P1.3, P1.2) as TXD and RXD. Here are the demos, including UART0_Init(), UART0_InChar(), UART0_OutChar(), UART0_OutString() and UART0_InString().
//------------UART0_Init------------ // Initialize the UART for 115,200 baud rate (assuming 12 MHz SMCLK clock), // 8 bit word length, no parity bits, one stop bit // Input: none // Output: none void UART0_Init(void){ EUSCI_A0->CTLW0 = 0x0001; // hold the USCI module in reset mode // bit15=0, no parity bits // bit14=x, not used when parity is disabled // bit13=0, LSB first // bit12=0, 8-bit data length // bit11=0, 1 stop bit // bits10-8=000, asynchronous UART mode // bits7-6=11, clock source to SMCLK // bit5=0, reject erroneous characters and do not set flag // bit4=0, do not set flag for break characters // bit3=0, not dormant // bit2=0, transmit data, not address (not used here) // bit1=0, do not transmit break (not used here) // bit0=1, hold logic in reset state while configuring EUSCI_A0->CTLW0 = 0x00C1; // set the baud rate // N = clock/baud rate = 12,000,000/115,200 = 104.1667 EUSCI_A0->BRW = 104; // UCBR = baud rate = int(N) = 104 EUSCI_A0->MCTLW &= ~0xFFF1; // clear first and second modulation stage bit fields P1->SEL0 |= 0x0C; P1->SEL1 &= ~0x0C; // configure P1.3 and P1.2 as primary module function EUSCI_A0->CTLW0 &= ~0x0001; // enable the USCI module EUSCI_A0->IE &= ~0x000F; // disable interrupts (transmit ready, start received, transmit empty, receive full) } //------------UART0_InString------------ // Accepts ASCII characters from the serial port // and adds them to a string until <enter> is typed // or until max length of the string is reached. // It echoes each character as it is input. // If a backspace is input, the string is modified // and the backspace is echoed // terminates the string with a null character // uses busy-waiting synchronization on RDRF // Input: pointer to empty buffer, size of buffer // Output: Null terminated string // -- Modified by Agustinus Darmawan + Mingjie Qiu -- void UART0_InString(char *bufPt, uint16_t max) { int length=0; char character; character = UART0_InChar(); while(character != CR){ if(character == BS){ if(length){ bufPt--; length--; UART0_OutChar(BS); } } else if(length < max){ *bufPt = character; bufPt++; length++; UART0_OutChar(character); } character = UART0_InChar(); } *bufPt = 0; } //------------UART0_InChar------------ // Wait for new serial port input // Input: none // Output: ASCII code for key typed char UART0_InChar(void){ while((EUSCI_A0->IFG&0x01) == 0); return((char)(EUSCI_A0->RXBUF)); } //------------UART0_OutChar------------ // Output 8-bit to serial port // Input: letter is an 8-bit ASCII character to be transferred // Output: none void UART0_OutChar(char letter){ while((EUSCI_A0->IFG&0x02) == 0); EUSCI_A0->TXBUF = letter; } //------------UART0_OutString------------ // Output String (NULL termination) // Input: pointer to a NULL-terminated string to be transferred // Output: none void UART0_OutString(char *pt){ while(*pt){ UART0_OutChar(*pt); pt++; } }
Using these five functions, we can send and receive messages through TXD and RXD. Use the Dupont Wires to attach the TXD and RXD with other pins, then you can use the software on Pi or your laptop to do serial port debugging. Another thing is that a common GND (Fig.32) is needed when you do the serial communication, so remember to connect the GND together on both boards.
You can use the CH340 USB (Fig.33) to serial chip to connect MSP432 to your laptop. For more information, you can see CH340_ETC.pdf. You can choose a serial port software to download from Google, for example, I use the Serial Port Utility (Fig.34).
It’s worth noticing that the serial port settings was set in UART0_Init() (Initialize the UART for 115,200 baud rate (assuming 12 MHz SMCLK clock), 8 bit word length, no parity bits, one stop bit.) After setting them, you can check the serial port by sending and receiving messages.
On the Pi, the serial function is not opened if you didn’t configure it. You can follow UART_config.docx to open UART on Pi ( Fig.35). If you have already finished it, you can jump this step.
The minicom (Fig.36) is a software under Linux operation system to test the serial ports. Print sudo apt-get install minicom and sudo minicom -D /dev/ttyAMA0 -b 115200 in command line to install it and open the serial port. Then you will open a window to send messages through this port, which is the same as your laptop. However, the software can only be used to do serial ports debugging. If you want to use the port in your program, you need the pyserial module.
The commands to install and import pyserial module are as follows:
$ sudo pip install pyserial $ python >>> import serial
Then you can call the functions of serial module to send and receive messages in your main program. Here is a demo.
import serial from time import sleep ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.5) def recv(serial): data while True: data =serial.read(30) if data == '': continue else: break sleep(0.02) return data while True: data =recv(ser) ser.write(data)
dwc_otg.lpm_enable=0 root=PARTUUID=a05c3c8f-02 rootfstype=ext4 elevator=deadline fsck.repair=yes rootwait quiet splash plymouth.ignore-serial-consoles
Reference: https://blog.csdn.net/RambleMY/article/details/81206090
There are many things which need to be noticed specially, because they can be easily set wrongly or may have serious influence on the whole system. Here, we list some problems we ever encountered and some important points.
In this project, we design and make a simple model to demonstrate the basic function of autopilot vehicle fleet. We use the components mainly offered by TI-RSLK and some other electronic devices such as Raspberry Pi, cameras, Wi-Fi modules and ultrasonic sensors. As a formation, the first vehicle has three ultrasonic sensors in different directions to detect and avoid obstacles automatically. It also can send commands directly to the other two vehicles by Wi-Fi modules as a server so that they can do the same actions simultaneously. This can realize in either remote or local network. To solve the deviation problem, we use a camera attached to Raspberry Pi 3B+ to recognize the symbol mounted on the back of the vehicle. The basic function has been realized, but it still has many aspects which worth being improved.