Introduction: Jousting Robot (LabVIEW Code)
This instructable will go over the LabVIEW code that I wrote to control a jousting robot. This will be an in-depth explanation of the code. If you're not interested in the explanation, the LabVIEW VI is linked on the last step.
For the robot, I used two continuous servos, a joystick, an accelerometer, a mini servo, and a force sensor. I also used the LabVIEW Physical Computing Kit which includes LabVIEW Home Bundle and chipKIT WF32. In order to communicate with the WF32 over its built-in WiFi, I used LabVIEW MakerHub LINX. To see a parts listing, check out the jousting robot wiring tutorial.
The overall idea for this project was to create a robot that could be controlled over WiFi and send data back to LabVIEW. First, there would be a setup state where you could set the angle of the "lance" mounted on a servo using the Pmod Joystick. Once a button on the PmodJSTK was pushed, the robot would enter a running state. In this state the servo motors would run until a force was sensed or if the robot was picked up. Once the robot was picked up, the state resets back to the setup state unless there have been 3 jousting rounds, in which case, the jousting session would end.
The robot has three hit-points to start with and each time during the running state if the force goes over a certain value, the hit-points decrease by one. If the hit-points decrease to zero, the jousting session will end.
Step 1: LINX and Sensor Initialization
The first part of the code is to initialize LINX and all of the sensors. In this case, LINX, the servos, the joystick, and the accelerometer need to be initialized.
Since the WF32 is connected via WiFi, the TCP Configuration control was used where the IP Address of the robot and the Port are specified. To set up WiFi using MakerHub LINX, check out this tutorial.
Next, the servos are initialized on pin 26, 27, and 28 and the SPI connection to PmodJSTK is created. PmodJSTK operates at 1MHz and sends data in Mode 0 with the MSb First.
PmodACL (the accelerometer) is set to have a range of ±4g and the default slave address 0x1D (Alt High).
The next part is to create the main loop for the program.
Step 2: Main Loop and States
For the main loop, we want to operate until the health reaches 0 or until the rounds remaining reaches 0. We also need to have at least three different states.
The setup state will be first which will lead into the run state and once the run state is over, we need to return to the setup state unless the remaining rounds is 0. In this case, we go into the stop state and end the main loop.
To set up the states, I used a enum constant that contains "Setup", "Run", and "Stop" states. A shift register was used so that after a state, an enum constant with next state can be sent into the right side shift register and then this value would be fed into the main case structure to choose the correct state. A shift register was also used for the health and rounds remaining values so that these could be tracked and modified between states.
Once the stop state is selected, a true value is sent to the main while loop in order to end the program.
Step 3: State 1: Setup Part 1
For the first loop iteration, the Setup value is sent into the case structure. This results in the setup state pictured above.
PmodJSTK had the CS pin connected to channel 10 and is active low. There is currently a bug in LINX SPI read that requires active high to be selected for communication to occur but this should be fixed shortly. The PmodJSTK also sends and receives 5 bytes. For the received bytes, the first two bytes together determine the y - axis values and the second two bytes together determine the x - axis values. These values range from 0 to 1024 (10 bit resolution). A subset array block was used to cut off the extra 6 bits that result when combining the two bytes together for both the x and y axes.
These integer values from the PmodJSTK are then sent to two individual case structures that measure whether the PmodJSTK is being controlled left, right, up, or down. If the PmodJSTK is at its origin, a "Nothing" is displayed. When the joystick is left, the values range from 512 to 1024 and when the joystick is right, the values range from 0 to 512.
Step 4: State 1: Setup Part 2
The servo used ranges from 500 uS to 2200 uS pulses. The servo points left with a value of 500 uS and points right with a 2200 uS pulses. The integer value was transformed into a desired pulse width such that when the PmodJSTK was turned left, the servo would turn left and when the PmodJSTK was turned right, the servo would turn right.
This was done by doing a linear calibration where the integer from PmodJSTK was multiplied by (500-2200) and divided by the whole range value of 1024. 2200 was then added to make a correct linear calibration.
Turned all the way to the left, the joystick will output 1024. Using the calibration, this will give a pulse width of 500uS which corresponds to the servo pointing left.
The last byte read from the PmodJSTK contains information about the on-board buttons. If none of the buttons are pushed, this value is 0. If one or all of the buttons are pushed, this value is non-zero. Thus the setup state is ended when a button on the Pmod is pushed because the resulting byte will be non-zero, resulting in a false value from the comparison block. The "not" block was used to switch this value to true and then the true value is sent to stop the while loop. The true value is also fed into a case structure after the while loop that sends the "Run" state into the main case structure using the shift register.
A false value is also sent to the main loop so that it continues to run.
Step 5: State 2: Run Part 1
The first part of this state is a 2000ms delay. This was used so that the user had time to remove their hand from the joystick before the robot would begin to drive.
After this, the servo write N channel block is used to set the servo motors forward by supplying a 2200uS pulse width to the left motor and 500uS pulse to the right motor. The pulse widths are different because one of the servos is "backwards."
From here, a while loop is used to monitor the force sensed and the acceleration value. From the data sheet, the force from the force sensor can be approximated for low forces using the equation F = 0.0389*exp(1.7727*V) where F is the force measurement and V is the voltage reading. A case structure was used so that when the voltage reading across the force sensor is zero, the force reading also displays zero. To make sure small voltage fluctuations are not included in the force reading, a select function is used so that small voltages result in a force reading of zero.
The next idea was to read the force measured by the sensor and display the max value after the while loop was over by allowing indexing the force measurement and then using a min/max block to read the max value of the resulting array.
Step 6: State 2: Run Part 2
In the while loop, if a sufficient force is sensed from the analog read block (voltage reading greater than 0.2V), a true value is sent to an "and" block. This "and" block is also connected to a shift register and the starting value is a false constant. This structure is used so that there aren't multiple true readings once the force value exceeds the threshold. At first, a false value is sent to the and structure and the voltage is below 0.2V.
When the voltage goes above 0.2V, a true value is sent into the "and" and the previous value (false) is inverted using the "not" which gives a true value into the "and" as well. Since the "and" block received two true constants,it sends a true value into the case structure.
For the next loop iteration, the voltage value will most likely still be greater than 0.2V, but the true value from the previous iteration is sent into the shift register and inverted to give a false constant. Thus the "and" block reads T and F which results in the false case. Once the voltage value goes below 0.2V, we're back to the original setup.
Once the "and" block sends a true value, the hit points are reduced by 1 and the servos are stopped to avoid damaging the robot.
If false (pictured above), the resources are just fed through the case structure.
Once the accelerometer (oriented upside-down) reads an acceleration less than -1.5 g (robot is picked up), a true value is sent to the stop button in order to end the while loop.
On a related note, since there may be voltage fluctuations when first reading the accelerometer, 10 loop iterations must pass before the acceleration can end the while loop. This was done by using another select function to return F when the loop iteration count is under 10 and then return the true or false value of the acceleration being less than -1.5g after 10 iterations.
Step 7: State 2: Run Part 3
In case the robot was not hit and thus it kept running, after the while loop is over (robot is picked up), the pulse widths are set to turn off the servos. The max force for that run is also recorded and this value is indexed into the main loop so that the max value for all of the runs can be measured as well.
Upon entering the run state, the rounds remaining constant is reduced by one.
If the rounds remaining is equal to zero or if the health is less than zero, the case structure feeds the "Stop" state to the shift register. Otherwise, the "Setup" state is returned to.
A false constant is also sent to the main loop so that it keeps running.
Step 8: State 3: Stop and Communication Close
In this state, all required resources are fed back into the shift registers and a True constant is sent to the main loop stop. This ends the program.
After the main loop is over, all that is left is to close the sensors and LINX. The accelerometer communication, servo communication, and LINX communication are all closed and the max force from each run is indexed out of the main loop and the max force from all runs is reported.
Step 9: Code
Here is the LabVIEW VI that I created. If you have any questions, please let me know in the comments.