diff --git a/AUTHORS-Platform.md b/AUTHORS-Platform.md index e69de29..e62a68f 100644 --- a/AUTHORS-Platform.md +++ b/AUTHORS-Platform.md @@ -0,0 +1,93 @@ +# P2-BLDC-Motor-Control - The Authors' Test Platform +as I work on these drivers I need to use a test platform. This page describes what my platofrm looks like and how it is set up. + +![Project Maintenance][maintenance-shield] + +[![License][license-shield]](LICENSE) + +## Images of my Platform + +I decided to use the inset wheels design (See [Drawings](DRAWINGS.md)) for mine and ordered it from [SendCutSend.com](https://sendcutsend.com/). I ordered a bunch of the spacers too. The order arrived like this: + +![Unboxing platform v2](images/unboxing-platform.jpg) + +You can see there are a number of predrilled holes for screws, castors, and cable feed through for the motor cables: + +- The 4 center small holes (for M3 screws) are for mounting the twin motors boards connected to the Mini Edge Breakout Board. +- 2 ea. holes for each motor (for 1/4" - 20 Screws.) +- 2x - 4 ea. holes for each castor - (for 1/4" - 20 Screws.) +- 2x - 2 ea. holes for each spacer board (shim) (for 1/4" - 20 Screws.) + +--- + + +After unboxing the parts need to be mounted: + +![Assembling the platform v2](images/building-the-platform.JPG) + +The pencil marks show where I drilled additional holes as I determined what additional parts I needed to mount. + +--- + +The finished platform looks like this: + +![Finished Platform](images/assembled-platform.JPG) + +Here are links to key parts I used: + +- [HRB 5S 18.5V 5000mAh 50-100C RC Lipo Battery](https://www.amazon.com/HRB-5000mAh-50-100C-Battery-T-REX550/dp/B06XP7TY3S) - Amazon +- [6-Position 45A Power Pole Distribution Block](https://www.amazon.com/Chunzehui-6-Position-Distribution-Connector-Distributor/dp/B07KQD9V3G) - Amazon +- [DC 12v 24v to 5v Step Down Converter Regulator 5A 25W](https://www.amazon.com/Converter-Regulator-Adapter-Reducer-Electronics/dp/B07Q5W1BG3) - Amazon - Added Anderson power-pole connector on input side and N barrel plug on output side. +- [150A Power Analyzer, High Precision RC with Digital LCD Screen](https://www.amazon.com/ANKG-Precision-Measurement-Connectors-connected/dp/B07YF393ZH) - Amazon, LCD screen shows V, A, W, Ah, Wh measurements +- [Inline Blade-Fuse Holders](https://www.amazon.com/iGreely-Terminals-Connectors-Automotive-Compatible/dp/B07ST82H9H) - Amazon, Replaced ring-terminal ends with XT90 socket to mate with battery + +**Castors** Special Note: the set screws tend to come lose with vibration. I was driving on gravel. So use loctite or something similar to hold them in place if you don't want castors falling off as your are driving. + +--- + +And here I'm running tests of the FlySky remote control demo. (Of course, running on the test-bench required an empty amazon box. ;-) + +![Driving with FlySly](images/drivingWithFlySky.PNG) + +--- + +Here's an updated bech-setup with the RPi in the drivers' seat! + +![Now with RPi in drivers' seat](images/more-complex-hasRPi.JPG) + +**NOTE:** the nVolt to 5V DC step-down supply was chosen so that I can power the P2 as well as the RPi from the same 5v. This picture shows me using a Y-splitter on the 5v with the two legs then feeding the P2 and the RPi. + + +--- + +> If you like my work and/or this has helped you in some way then feel free to help me out for a couple of :coffee:'s or :pizza: slices! +> +> [![coffee](https://www.buymeacoffee.com/assets/img/custom_images/black_img.png)](https://www.buymeacoffee.com/ironsheep)    -OR-    [![Patreon](./images/patreon.png)](https://www.patreon.com/IronSheep?fan_landing=true)[Patreon.com/IronSheep](https://www.patreon.com/IronSheep?fan_landing=true) + +--- + +## Disclaimer and Legal + +> *Parallax, Propeller Spin, and the Parallax and Propeller Hat logos* are trademarks of Parallax Inc., dba Parallax Semiconductor + +--- + +## License + +Copyright © 2022 Iron Sheep Productions, LLC. All rights reserved. + +Licensed under the MIT License. + +Follow these links for more information: + +### [Copyright](copyright) | [License](LICENSE) + +[maintenance-shield]: https://img.shields.io/badge/maintainer-stephen%40ironsheep%2ebiz-blue.svg?style=for-the-badge + +[marketplace-version]: https://vsmarketplacebadge.apphb.com/version-short/ironsheepproductionsllc.spin2.svg + +[marketplace-installs]: https://vsmarketplacebadge.apphb.com/installs-short/ironsheepproductionsllc.spin2.svg + +[marketplace-rating]: https://vsmarketplacebadge.apphb.com/rating-short/ironsheepproductionsllc.spin2.svg + +[license-shield]: https://camo.githubusercontent.com/bc04f96d911ea5f6e3b00e44fc0731ea74c8e1e9/68747470733a2f2f696d672e736869656c64732e696f2f6769746875622f6c6963656e73652f69616e74726963682f746578742d646976696465722d726f772e7376673f7374796c653d666f722d7468652d6261646765 diff --git a/DOCs/sources/objects.graffle b/DOCs/sources/objects.graffle index 6c7e51c..1842e4e 100644 Binary files a/DOCs/sources/objects.graffle and b/DOCs/sources/objects.graffle differ diff --git a/DRIVE-OBJECTS-SERIAL.md b/DRIVE-OBJECTS-SERIAL.md new file mode 100644 index 0000000..c6bac7e --- /dev/null +++ b/DRIVE-OBJECTS-SERIAL.md @@ -0,0 +1,122 @@ + +# P2-BLDC-Motor-Control - Serial Interface of Steering Object + +Serial interface for our steering object - P2 Spin2/Pasm2 for our 6.5" Hub Motors with Universal Motor Driver Boards + +![Project Maintenance][maintenance-shield] + +[![License][license-shield]](LICENSE) + +There are two objects in our motor control system. There is a lower-level object (**isp\_bldc_motor.spin2**) that controls a single motor and there's an upper-level object (**isp\_steering_2wheel.spin2**) which coordinates a pair of motors as a drive subsystem. + +This document describes the serial interface of the steering object which controls a dual motor robot platform. + +The drive subsystem currently uses two cogs, one for each motor. A third cog privides motor position sense.  Conceptually, the drive system is always running. It is influenced by updating control variable values. When the values change the drive subsystem responds accordingly. The public methods of the steering object simply writes to these control variables and/or read from associated status variables of the sensor cog returning their current value or an interpretation thereof. + +## Object: isp\_steering_2wheel.spin2 + +This steering object makes it easy to make your robot drive forward, backward, turn, or stop. You can adjust the steering to make your robot go straight, drive in arcs, or make tight turns. This steering object is for robot vehicles that have two motors, with one motor driving the left side of the vehicle and the other the right side. + + +### The 2-Wheel Steering Object PUBLIC Interface + +With each public method of this object we've added the serial command and response values. in this example: + +``` +SER drivedir {pwr} {dir} +SER Returns: OK | ERROR {errormsg} +``` + +... we see the first line represents the command to be sent from the connected device (RPi, Arduino, etc.) to the P2 via serial. On the second line we see "Returns:" meaning that the P2 upon receiving the command will return the specified response. + +Generally speaking when driving a robot using these serial commands one expects to see: + +``` +commandA +ok +commandB +ok +commandC +ok +``` + +... the non OK responses are there to help us detect problems early in the development of our control routines. We don't expect to be getting non-OK responses once we've developed our drive code. The exception to this would be if our drive code generates values which would be out of the range of legal values for a given command. + + +The object **isp\_steering_2wheel.spin2** provides the following methods: + +| Steering Interface | Description | +| --- | --- | +| **>--- CONTROL** +| 
PUB driveDirection(power, direction)

SER drivedir {pwr} {dir}
SER Returns: OK \| ERROR {errormsg}
| Control the speed and direction of your robot using the {power} and {direction} inputs.
Turns both motors on at {power, [(-100) to 100]} but adjusted by {direction, [(-100) to 100]}.
AFFECTED BY: setAcceleration(), setMaxSpeed(), holdAtStop() +| 
PUB driveForDistance(leftDistance, rightDistance, distanceUnits)

SER drivedist {ltdist} {rtdist} {d-u}
SER Returns: OK \| ERROR {errormsg}
| Control the forward direction or rate of turn of your robot using the {leftDistance} and {rightDistance} inputs.
Turn both motors on then turns them off again when either reaches the specified {leftDistance} or {rightDistance}, where {\*distance} is in {distanceUnits} [DDU\_IN, DDU\_CM, DDU\_FT or DDU\_M].
AFFECTED BY: setAcceleration(), setMaxSpeedForDistance(), holdAtStop() +| PUB driveAtPower(leftPower, rightPower)
SER drivepwr {ltpwr} {rtpwr}
SER Returns: OK \| ERROR {errormsg}
| Control the speed and direction of your robot using the {leftPower} and {rightPower} inputs.
Turns left motor on at {leftPower} and right on at {rightPower}. Where {\*Power} are in the range [(-100) to 100].
AFFECTED BY: setAcceleration(), setMaxSpeed(), holdAtStop() +| PUB stopAfterRotation(rotationCount, rotationUnits)
SER stopaftrot {count} {r-u}
SER Returns: OK \| ERROR {errormsg}
| Stops both motors, after either of the motors reaches {rotationCount} of {rotationUnits} [DRU\_DEGREES, DRU\_ROTATIONS, or DRU\_HALL_TICKS].
USE WITH: driveDirection(), drive() +| PUB stopAfterDistance(distance, distanceUnits)
SER stopaftdist {dist} {d-u}
SER Returns: OK \| ERROR {errormsg}
| Stops both motors, after either of the motors reaches {distance} specified in {distanceUnits} [DDU\_IN, DDU\_CM, DDU\_FT or DDU\_M].
USE WITH: driveDirection(), drive() +| PUB stopAfterTime(time, timeUnits)
SER stopafttime {time} {t-u}
SER Returns: OK \| ERROR {errormsg}
| Stops both motors, after {time} specified in {timeUnits} [DTU\_IN\_MILLISEC or DTU\_IN\_SEC] has elapsed.
USE WITH: driveDirection(), drive() +| PUB stopMotors()
SER stopmotors
SER Returns: OK
| Stops both motors, killing any motion that was still in progress
AFFECTED BY:holdAtStop() +| PUB emergencyCutoff()
SER emercutoff
SER Returns: OK
| EMERGENCY-Stop - Immediately stop both motors, killing any motion that was still in progress +| PUB clearEmergency()
SER emerclear
SER Returns: OK
| clear the emergency stop status allowing motors to be controlled again +| **>--- CONFIG** +| PUB start(eLeftMotorBasePin, eRightMotorBasePin, eMotorVoltage)
SER N/A
| Specify motor control board connect location for each of the left and right motor control boards +| PUB stop() 
SER N/A
| Stop cogs and release pins assigned to motor drivers +| PUB setAcceleration(rate)
SER setaccel {rate}
SER Returns: OK \| ERROR {errormsg}
| **NOT WORKING, YET**
Limit Acceleration to {rate} where {rate} is [??? - ???] mm/s squared (default is ??? mm/s squared) +| PUB setMaxSpeed(speed)
SER setspeed {speed}
SER Returns: OK \| ERROR {errormsg}
| Limit top-speed to {speed} where {speed} is [1 to 100] - *DEFAULT is 75 and applies to both forward and reverse* +| PUB setMaxSpeedForDistance(speed)
SER setspeedfordist {speed}
SER Returns: OK \| ERROR {errormsg}
| Limit top-speed of driveDistance() operations to {speed} where {speed} is [1 to 100] - *DEFAULT is 75 and applies to both forward and reverse* +| PUB calibrate() 
SER N/A
| **NOT WORKING, YET**
*(we may need this?)* +| PUB holdAtStop(bEnable)
SER hold {true \| false}
SER Returns: OK \| ERROR {errormsg}
| Informs the motor subsystem to actively hold postiion (bEnable=true) or coast (bEnable=false) at end of motion +| PUB resetTracking()
SER resettracking
SER Returns: OK
| Resets the position tracking values returned by getDistance() and getRotations() +| **>--- STATUS** +| PUB getDistance(distanceUnits) : leftDistanceInUnits, rightDistanceInUnits
SER getdist {d-u}
SER Returns: dist {ltDistInUnits} {rtDistInUnits} \| ERROR {errormsg}
| Returns the distance in {distanceUnits} [DDU\_IN, DDU\_CM, DDU\_FT or DDU\_M] travelled by each motor since last reset +| PUB getRotationCount(rotationUnits) : leftRotationCount, rightRotationCount 
SER getrot {r-u}
SER Returns: rot {ltRotCountInUnits} {rtRotCountInUnits} \| ERROR {errormsg}
| Returns accumulated {*RotationCount} in {rotationUnits} [DRU\_DEGREES, DRU\_ROTATIONS, or DRU\_HALL_TICKS], since last reset, for each of the motors. +| PUB getPower() : leftPower, rightPower 
SER getpwr
SER Returns: pwr {ltPwr} {rtPwr}
| Returns the last specified power value for each of the motors (will be zero if the motor is stopped). +| PUB getStatus() : eLeftStatus, eRightStatus
SER getstatus
SER Returns: stat {ltStatus} {rtStatus}
| Returns status of motor drive state for each motor: enumerated constant: DS\_MOVING, DS\_HOLDING, DS\_OFF, or DS_Unknown +| PUB getMaxSpeed() : maxSpeed 
SER getmaxspd
SER Returns: speedmax {maxSpeed}
| Returns the last specified {maxSpeed} +| PUB getMaxSpeedForDistance() : maxSpeed4dist 
SER getmaxspdfordist
SER Returns: speeddistmax {maxSpeed}
| Returns the last specified {maxSpeedForDistance} + +**NOTE1** {power} whenever used is [(-100) - 100] where neg. values drive backwards, pos. values forward, 0 is hold/stop + +**NOTE2** {direction} whenever used is [(-100) - 100] A value of 0 (zero) will make your robot vehicle drive straight. A positive number (greater than zero) will make the robot turn to the right, and a negative number will make the robot turn to the left. The farther the steering value is from zero, the tighter the turn will be. + +**NOTE3** A HALL TICK is 4° for our 6.5" Dia. Motors. + +**NOTE4** {e\*MotorBasePin} is one of: PINS\_P0\_P15, PINS\_P16\_P31, or PINS\_P32\_P47 + +**NOTE5** {eMotorVoltage} is one of: PWR\_7p4V, PWR\_11p1V, PWR\_12V, PWR\_14p8V, PWR\_18p5V, or PWR\_22p2V + + +### ... + +--- + +> If you like my work and/or this has helped you in some way then feel free to help me out for a couple of :coffee:'s or :pizza: slices! +> +> [![coffee](https://www.buymeacoffee.com/assets/img/custom_images/black_img.png)](https://www.buymeacoffee.com/ironsheep)    -OR-    [![Patreon](./images/patreon.png)](https://www.patreon.com/IronSheep?fan_landing=true)[Patreon.com/IronSheep](https://www.patreon.com/IronSheep?fan_landing=true) + +--- + +## Disclaimer and Legal + +> *Parallax, Propeller Spin, and the Parallax and Propeller Hat logos* are trademarks of Parallax Inc., dba Parallax Semiconductor + +--- + +## License + +Copyright © 2022 Iron Sheep Productions, LLC. All rights reserved. + +Licensed under the MIT License. + +Follow these links for more information: + +### [Copyright](copyright) | [License](LICENSE) + +[maintenance-shield]: https://img.shields.io/badge/maintainer-stephen%40ironsheep%2ebiz-blue.svg?style=for-the-badge + +[marketplace-version]: https://vsmarketplacebadge.apphb.com/version-short/ironsheepproductionsllc.spin2.svg + +[marketplace-installs]: https://vsmarketplacebadge.apphb.com/installs-short/ironsheepproductionsllc.spin2.svg + +[marketplace-rating]: https://vsmarketplacebadge.apphb.com/rating-short/ironsheepproductionsllc.spin2.svg + +[license-shield]: https://camo.githubusercontent.com/bc04f96d911ea5f6e3b00e44fc0731ea74c8e1e9/68747470733a2f2f696d672e736869656c64732e696f2f6769746875622f6c6963656e73652f69616e74726963682f746578742d646976696465722d726f772e7376673f7374796c653d666f722d7468652d6261646765 diff --git a/README.md b/README.md index aa46b16..31e681e 100644 --- a/README.md +++ b/README.md @@ -16,13 +16,17 @@ NOTE: *If you wish to add more than a couple of sensors to your platform then yo Parallax offers a pair of the [6.5" Hoverboard wheels along with mounting hardware](https://www.parallax.com/product/6-5-hub-motors-with-encoders-and-mounting-blocks-bundle/) which is perfect for use with the drivers from this project. -This post [Build a Heavy-Duty Robot Using 6.5″ Hub Motors and Propeller 2 Control System](https://www.parallax.com/build-a-heavy-duty-robot-using-brushless-dc-motors/) describes our two-wheel system. The objects provided by this project provide all you need to get your platform moving! +This post [Build a Heavy-Duty Robot Using 6.5″ Hub Motors and Propeller 2 Control System](https://www.parallax.com/build-a-heavy-duty-robot-using-brushless-dc-motors/) describes our two-wheel system. The objects provided by this project are all you need to get your platform moving! ## Current status Latest Changes: ``` +04 May 2022 v2.0.0 +- Add new Serial Top-Level object as interface to 2-wheel steering object. + -- Drive your platform from RPi or Arduino (RPi example provided) +- Fixed issues with position tracking and reporting within isp_steering_2wheel.spin2 27 Apr 2022 v1.1.0 - Add emergency stop methods to isp_bldc_motor.spin2, and isp_steering_2wheel.spin2 - Adjust spin-up ramp to start slower then speed up (better traction on loose surfaces but faster speed-up as well.) @@ -32,13 +36,27 @@ Latest Changes: - Initial Public Release ``` +## Known Issues + +Things we know about that still need attention: + +``` +v2.0.0 +- Drive status reporting is not working in base objects so is also reported badly over serial I/F +v1.1.0 +- Issues with position tracking not working in isp_steering_2wheel.spin2 +- Drive status reporting is not working in base objects (motor and steering) +``` + ## Table of Contents On this Page: - [Motor Object Introduction](https://github.com/ironsheep/P2-BLDC-Motor-Control#single-and-two-wheeled-motor-control-objects) - An overview of the objects provided by this project - [System Diagram](https://github.com/ironsheep/P2-BLDC-Motor-Control#system-diagram) - A quick visual overview of the motor and steering runtime structure +- [Future Directions](https://github.com/ironsheep/P2-BLDC-Motor-Control#future-directions) - Notes about areas we can improve over time if you have experience and want to help us with these, please contact us in Forums [(in this thread)](https://forums.parallax.com/discussion/174516/universal-motor-driver-dual-wheel-steering-motor-control-and-position-tracking-objects-for-p2#latest) - [DEMOs](https://github.com/ironsheep/P2-BLDC-Motor-Control#demos) - Example files that show how to interact with the motor control and steering objects provided by this project +- [FlySky Controls for DEMO](https://github.com/ironsheep/P2-BLDC-Motor-Control#flysky-controls-for-demo) - How our Switches and Joysticks are tasked - [Reference](https://github.com/ironsheep/P2-BLDC-Motor-Control#references) - We looked at a number of control systems before deciding on the public interfaces for our steering and motor control objects - [How to Contribute](https://github.com/ironsheep/P2-BLDC-Motor-Control#how-to-contribute) - We welcome contributions back to this project for all of us to use. Learn how, here! @@ -46,15 +64,16 @@ Additional pages: - [Steering and Motor control](DRIVE-OBJECTS.md) - The object public interfaces - [Start your drive project using these objects](DEVELOP.md) - Walks thru configuration and setup of your own project using these objects +- [Use RPi or Arduino to control your platform](SERIAL-CONTROL.md) - Walks thru configuration and setup of RPi control system (extrapolate to Arduino) - [Drawings](DRAWINGS.md) - Files (.dwg) that you can use to order your own platform inexpensively - [To-scale drawings](DOCs/bot-layout.pdf) of possible rectangular and round robotic drive platforms for Edge Mini Break and JonnyMac P2 Development boards - +- [The author's development platform](AUTHORS-Platform.md) - Overview of the robot platform used when developing and testing the code for this project ## Single and Two-wheeled Motor Control Objects There are two objects in our motor control system. There is a lower-level object (**isp\_bldc_motor.spin2**) that controls a single motor and there's an upper-level object (**isp\_steering_2wheel.spin2**) which coordinates a pair of motors as a drive subsystem. -If you are working with a dual motor device then you'll be coding to the interface of this upper-level steering object as you develop your drive algorithms.  If you were to work with say a three-wheeled device then you may want to create a steering object that can better coordinate all three motors at the same time. Actually this is true for any other number of motors you decide to control. Create your own better-suited steering object, base it on this project's 2-wheel version. (*And, if you do please consider contributing your work to this project so it can be available to us all! See: [How to Contribute](https://github.com/ironsheep/P2-BLDC-Motor-Control/tree/develop#how-to-contribute) below.*) +If you are working with a dual motor device then you'll be coding to the interface of this upper-level steering object as you develop your drive algorithms.  If you were to work with say a three-wheeled device then you may want to create a steering object that can better coordinate all three motors at the same time. Actually this is true for any other number of motors you decide to control. Create your own better-suited steering object, base it on this project's 2-wheel version. (*And, if you do, please consider contributing your work to this project so it can be available to us all! See: [How to Contribute](https://github.com/ironsheep/P2-BLDC-Motor-Control/tree/develop#how-to-contribute) below.*) The drive subsystem currently uses two cogs, one for each motor and a third cog for active tracking of wheel position. Conceptually, the drive system is always running. It is influenced by updating control variable values. When the values change the drive subsystem responds accordingly. The public methods of both the steering object and the motor object simply write to these control variables and/or read from associated status variables returning their current value or an interpretation thereof. @@ -70,6 +89,14 @@ The following diagram shows the nested motor control and sense subsystem compris In this diagram there are three **rectangular objects** depicting files (yellow background) of code. There are three methods within the files (white and green backgrounds) that are run in separate cogs. The **arrows** attempt to show which objects interact with each other and also show with which object the user application can interact. The gear icon indicates which are running in their own Cog. You can see that the users' top-level control application runs in its own Cog as well. +## Future directions + +We now have a working motor drive system that is fun to use. While this was being developed we kept track of further improvements that can be made in the future. Here's our list of the most notable: + +- Add acceleration control +- Replace simple built-in spin-up, spin-down ramps with better + + ## DEMOs A small number of demos are provided with this project: @@ -119,6 +146,10 @@ This is a project supporting our P2 Development Community. Please feel free to c --- +## Credits + +This work is entirely based upon **Chip Gracey**'s excellent **BLDC Motor Driver written in Pasm2** which he demonstrated to us in many of our [live forums](https://www.youtube.com/playlist?list=PLt_MJJ1F_EXa25_TWa4Sdi77daQaxA_ZU). This project allows us all to use this driver. Thank you Chip! + ## Disclaimer and Legal > *Parallax, Propeller Spin, and the Parallax and Propeller Hat logos* are trademarks of Parallax Inc., dba Parallax Semiconductor diff --git a/SERIAL-CONTROL.md b/SERIAL-CONTROL.md new file mode 100644 index 0000000..3e13bdd --- /dev/null +++ b/SERIAL-CONTROL.md @@ -0,0 +1,184 @@ +# P2-BLDC-Motor-Control - Via Serial from RPi, Arduino, or... + +Serial control of our Single and Two-motor driver objects P2 Spin2/Pasm2 for our 6.5" Hub Motors with Universal Motor Driver Board + +![Project Maintenance][maintenance-shield] + +[![License][license-shield]](LICENSE) + +## The Project + +Instead of using the FlySky for remote control this document describes how to use a 2-wire serial interface to control your P2 hardware on your robot platform. + +The code for this project implements an active serial receiver running on the P2 and a top-level application which listens for drive/status commands arriving via serial and then forwards the requests to the 2-wheel steering system and sends responses back over serial to the host. + +## Current status + +Latest Changes: + +``` +04 May 2022 v2.0.0 +- Initial Public Release of Serial support +``` + +## Table of Contents + +On this Page: + +- [System Diagram](#system-diagram) - what are we setting up? +- [Download the latest files](#download-the-project-files) - get latest project files +- [Configure the RPi](#configuring-your-rpi) - one time configure your RPi +- [Wiring the Serial Connection](#wiring-the-serial-connection) - Connect the RPi to the P2 +- [Flashing your P2](#flashing-your-p2-edge) - flash to P2 with the drive code +- [Building your own drive code](#developing-your-own-drive-code) - write your own drive code! + +Additional pages: + +- [README](README.md) - The top level file for this repository +- [Steering and Motor control](DRIVE-OBJECTS.md) - The object public interfaces +- [Start your drive project using these objects](DEVELOP.md) - Walks thru configuration and setup of your own project using these objects +- [Drawings](DRAWINGS.md) - Files (.dwg) that you can use to order your own platform inexpensively +- [To-scale drawings](DOCs/bot-layout.pdf) of possible rectangular and round robotic drive platforms for Edge Mini Break and JonnyMac P2 Development boards +- [The author's development platform](AUTHORS-Platform.md) - Overview of the robot platform used when developing and testing the code for this project + + +--- + +## System Diagram + +The following diagram shows the top-level serial object which hands off commands to the drive subsystem as they are received. It also shows the nested motor control and sense subsystem comprised of the two objects: steering and motor control. + +![Motor Control System Diagram](./images/serial-bldc-system.png) + +In this diagram there are five **rectangular objects** depicting files (yellow background) of code. There are three methods within the motor files (white and green backgrounds) that are run in separate cogs and there is one method (white background) within the serial files that runs in a separate cog. The **arrows** attempt to show which objects interact with each other and also show with which object the user application can interact. The gear icon indicates which are running in their own Cog. You can see that the users' top-level control application runs in its own Cog as well. + +## Download the project files + +Head to the BLDC Motor repository [releases page](https://github.com/ironsheep/P2-BLDC-Motor-Control/releases) and download the `serial-control-archive-set.zip` file from the Assets section of the latest release. *(only present in v2.0.0 and later releases)* + +Create a working directory and unpack this .zip there. In this .zip file you'll find an archive (.zip) of the P2 project files you need and a `pythonSrc.zip` of RPi side files you need. We'll use all of these files in later steps. Meanwhile, let's setup your RPi. + +## Configuring your RPi + +Set up for your RPi is nearly the same as we did for setting up the RPi for P2 IoT Gateway use. + +But, first, if you are configuring a bare new RPi then do: [Setting up your 1st Raspberry Pi](https://github.com/ironsheep/P2-RPi-IoT-gateway/blob/main/RPI-SETUP.md) + +If you have a well setup RPi and just need to configure it for use in this contect then do the following... + +First install extra packages the script needs: + +### Packages for Ubuntu, Raspberry pi OS, and the like + +```shell +sudo apt-get install git python3 python3-pip python3-tzlocal python3-sdnotify python3-colorama python3-unidecode python3-paho-mqtt python3-watchdog +``` + +### Finish the project install on the RPi + +You need to select a location to install your platform-drive python script. + +If you were installing in say your home directory/projects/ it might look something like this: + +```shell +cd # insure you are in home directory + +mkdir -p ~/projects/platform-drive # make new directory (inclu. ~/projects/ if it doesn't exist) + +cd ~/projects/platform-drive +``` + +Head back to the folder where you unpacked the `serial-control-archive-set.zip` file. Let's unpack the `pythonSrc.zip` file found within. Now copy the files from the newly created ./pythnSrc folder into this new directory on your RPi. One if the files should be a `requirements.txt` file and the other should be the demo script `P2-BLDC-Motor-Control-Demo.py`. Finish up your system prep by ensuring the files needed for you drive script are installed with the following commands: + +```shell +cd ~/projects/platform-drive # make sure we are where the new files arrived +sudo pip3 install -r requirements.txt # install supporting files +``` + + +## Wiring the Serial Connection + +The **P2-BLDC-Motor-Control-Demo.py** script is built to use the main serial I/O channel at the RPi GPIO Interface. These are GPIO pins 14 & 15 (header pins 8 & 10). + +**NOTE:** FYI a good reference is: [pinout diagram for RPi GPIO Pins](https://pinout.xyz/) + +**RPi Wiring for Daemon use:** + +| RPi Hdr Pin# | RPi GPIO Name| RPi Purpose | P2 Purpose | P2 Pin # | +| --- | --- | --- | --- | --- | +| 6 | GND | Signal ground| Signal ground | GND near Tx/Rx Pins| +| 8 | GPIO 14 | Uart Tx | Serial Rx (from RPi) | 57 +| 10 | GPIO 15 | Uart Rx | Serial Tx (to RPi) | 56 + +Pick two pins on your P2 dev board to be used for RPi serial communications. The top-level file provided by this project defines these two pins as 56, 57. This was due to the two motor control boards occupying most of the remaining pins on the Mini Edge Breakout board. Feel free to choose different pins. Just remember to adjust the constants in your code to use your pin choices. + +## Flashing your P2 Edge + +The code for this project is setup for the P2 Mini Edge Breakout board with the two BLDC motor controllers installed at each of the dual-header locations. This leave one single header where you just connected the serial wires from the RPi. + +Head back to the folder where you unpacked the `serial-control-archive-set.zip` file. Let's unpack the *archive.zip file found within. + +In the subfolder just created locate the `isp_steering_serial.spin2` file. Using Propeller tool select this file as our top-level file then compile and download to FLASH. + +**NOTE:** *this top-level file `isp_steering_serial.spin2` contains all pin-mappings. If you are not set up as the demo specified then make your pinout changes here before you flash the code.* + +This should be all you need to test your new installation. The demo file will drive the motors as if the platform is driving in a square (drive straight, turn right, drive strait, turn right, etc.) + +You can test by running the following command on your RPi: + +```shell +cd ~/projects/platform-drive # make sure we are your project files are +./P2-BLDC-Motor-Control-Demo.py -d -v # run with debug and verbose messaging enabled +``` + +If this is working for you, congratulations, you are all set up and ready to do your own drive code! + + +## Developing your own drive code + +On your RPi copy the `P2-BLDC-Motor-Control-Demo.py` file to your own name: + +```shell +cd ~/projects/platform-drive # make sure we are your project files are +cp /P2-BLDC-Motor-Control-Demo.py {newName}.py # create new copy +``` + +Do all your development in this new file. Start by replacing the sqaure-pattern drive code with your own. + +Have fun! + +### ... + +--- + +> If you like my work and/or this has helped you in some way then feel free to help me out for a couple of :coffee:'s or :pizza: slices! +> +> [![coffee](https://www.buymeacoffee.com/assets/img/custom_images/black_img.png)](https://www.buymeacoffee.com/ironsheep)    -OR-    [![Patreon](./images/patreon.png)](https://www.patreon.com/IronSheep?fan_landing=true)[Patreon.com/IronSheep](https://www.patreon.com/IronSheep?fan_landing=true) + +--- + +## Disclaimer and Legal + +> *Parallax, Propeller Spin, and the Parallax and Propeller Hat logos* are trademarks of Parallax Inc., dba Parallax Semiconductor + +--- + +## License + +Copyright © 2022 Iron Sheep Productions, LLC. All rights reserved. + +Licensed under the MIT License. + +Follow these links for more information: + +### [Copyright](copyright) | [License](LICENSE) + +[maintenance-shield]: https://img.shields.io/badge/maintainer-stephen%40ironsheep%2ebiz-blue.svg?style=for-the-badge + +[marketplace-version]: https://vsmarketplacebadge.apphb.com/version-short/ironsheepproductionsllc.spin2.svg + +[marketplace-installs]: https://vsmarketplacebadge.apphb.com/installs-short/ironsheepproductionsllc.spin2.svg + +[marketplace-rating]: https://vsmarketplacebadge.apphb.com/rating-short/ironsheepproductionsllc.spin2.svg + +[license-shield]: https://camo.githubusercontent.com/bc04f96d911ea5f6e3b00e44fc0731ea74c8e1e9/68747470733a2f2f696d672e736869656c64732e696f2f6769746875622f6c6963656e73652f69616e74726963682f746578742d646976696465722d726f772e7376673f7374796c653d666f722d7468652d6261646765 diff --git a/demo_dual_motor_rc.spin2 b/demo_dual_motor_rc.spin2 index 46914e3..9482021 100644 --- a/demo_dual_motor_rc.spin2 +++ b/demo_dual_motor_rc.spin2 @@ -7,7 +7,7 @@ '' -- see below for terms of use '' E-mail..... stephen@ironsheep.biz '' Started.... Mar 2022 -'' Updated.... 19 Mar 2022 +'' Updated.... 04 May 2022 '' '' ================================================================================================= @@ -159,9 +159,14 @@ VAR long nRtAmps long nRtWatts -PUB remoteControlMotor() | bLastIgnoreJoyStks, bIgnoreJoySticks, bDoneTesting, bLastShowState, bShowState, loopCt, bShowDebugStatus, bEmerCutoff, bLastEmerCutoff + long bLastShowState + long bLastEmerCutoff + long bLastIgnoreJoyStks + long bLastShowDriveStates - waitms(500) ' wait for 3 sec for backend to catch up +PUB remoteControlMotor() | bIgnoreJoySticks, bDoneTesting, bShowState, loopCt, bShowDebugStatus, bEmerCutoff, bShowDriveStates +'' Listen to FlySky and control motor, get status based on Switch/Joystick inputs + waitms(500) ' wait for 1/2 sec for backend to catch up 'arm.gripClosed() debug("------") @@ -179,24 +184,32 @@ PUB remoteControlMotor() | bLastIgnoreJoyStks, bIgnoreJoySticks, bDoneTesting, b bLastShowState := VALUE_NOT_SET ' value can't happen bLastIgnoreJoyStks := VALUE_NOT_SET ' value can't happen bLastEmerCutoff := VALUE_NOT_SET ' value can't happen + bLastShowDriveStates := VALUE_NOT_SET ' value can't happen loopCt := 0 repeat 'debug("* ", udec_long(loopCt)) - bShowDebugStatus := loopCt < 20 ? TRUE : FALSE + bShowDebugStatus := FALSE 'loopCt < 2 ? TRUE : FALSE 'remoteCtl.showDebug(bShowDebugStatus) ' SwA Ignore JoySticks - debug("-- -- SW -- --") + 'debug("-- -- SW -- --") bIgnoreJoySticks := remoteCtl.swIsOn(remoteCtl.CTL_SW_A) bShowState := remoteCtl.swIsOff(remoteCtl.CTL_SW_B) ' SwC done, end loop! bDoneTesting := remoteCtl.swIsOff(remoteCtl.CTL_SW_C) + bShowDriveStates := remoteCtl.swIsMiddle(remoteCtl.CTL_SW_C) ' SwD down = Emergency Cutoff remoteCtl.showDebug(bShowDebugStatus) bEmerCutoff := remoteCtl.swIsOff(remoteCtl.CTL_SW_D) remoteCtl.showDebug(FALSE) + if bLastShowDriveStates <> bShowDriveStates + debug("- sho ", sdec_long_(bShowDriveStates)) + if bShowDriveStates + wheels.showDriveStates() + bLastShowDriveStates := bShowDriveStates + if bShowState <> bLastShowState 'showServoPositions() if bShowState @@ -252,7 +265,7 @@ PUB remoteControlMotor() | bLastIgnoreJoyStks, bIgnoreJoySticks, bDoneTesting, b PRI updPosnBothJoy(): speed, direction | rawSpeed, rawDirection, rngMin, rngMax ' route Rt Joystick Vt (spring center) to FWD/REV (speed) - debug("-- JOY -- JOY --") + 'debug("-- JOY -- JOY --") rawSpeed := remoteCtl.readChannel(remoteCtl.CTL_RT_JOY_VT) rngMin, rngMax := remoteCtl.chanMinMax(remoteCtl.CTL_RT_JOY_VT) speed := map(rawSpeed, rngMin, rngMax, -100, +100) diff --git a/get b/get index 833b305..87f8326 100755 --- a/get +++ b/get @@ -1,7 +1,11 @@ #!/bin/bash +SRC_PYTHON_DIR=/home/pi/Projects/P2-BLDC-Control +DST_PYTHON_DIR="/Users/stephen/Projects/Projects-ExtGit/IronSheepProductionsLLC/Projects Propeller/P2-BLDC-Motor-Control/P2-BLDC-Motor-Control/pythonSrc" + SRC_SPIN_DIR=/Users/stephen/Dropbox/PropV2-Shared/Projects/P2-BLDC-Motor-ControlSW DST_SPIN_DIR="/Users/stephen/Projects/Projects-ExtGit/IronSheepProductionsLLC/Projects Propeller/P2-BLDC-Motor-Control/P2-BLDC-Motor-Control" (set -x;cp -p "${SRC_SPIN_DIR}"/*.spin2 "${DST_SPIN_DIR}") (set -x;cp -p "${SRC_SPIN_DIR}"/p2font16 "${DST_SPIN_DIR}") +(set -x;scp -p pi@pip2iotgw-wifi.home:"${SRC_PYTHON_DIR}"/* "${DST_PYTHON_DIR}") diff --git a/images/assembled-platform.JPG b/images/assembled-platform.JPG new file mode 100644 index 0000000..3eec796 Binary files /dev/null and b/images/assembled-platform.JPG differ diff --git a/images/building-the-platform.JPG b/images/building-the-platform.JPG new file mode 100644 index 0000000..bd2decc Binary files /dev/null and b/images/building-the-platform.JPG differ diff --git a/images/drivingWithFlySky.PNG b/images/drivingWithFlySky.PNG new file mode 100644 index 0000000..09480d2 Binary files /dev/null and b/images/drivingWithFlySky.PNG differ diff --git a/images/more-complex-hasRPi.JPG b/images/more-complex-hasRPi.JPG new file mode 100644 index 0000000..61dd9c5 Binary files /dev/null and b/images/more-complex-hasRPi.JPG differ diff --git a/images/serial-bldc-system.png b/images/serial-bldc-system.png new file mode 100644 index 0000000..49f17bb Binary files /dev/null and b/images/serial-bldc-system.png differ diff --git a/images/unboxing-platform.jpg b/images/unboxing-platform.jpg new file mode 100644 index 0000000..2196f4c Binary files /dev/null and b/images/unboxing-platform.jpg differ diff --git a/isp_bldc_motor.spin2 b/isp_bldc_motor.spin2 index a9a8750..2835359 100644 --- a/isp_bldc_motor.spin2 +++ b/isp_bldc_motor.spin2 @@ -114,7 +114,7 @@ PUB start(eMotorBasePin, eMotorVoltage) : ok | legalBase ' init status variables loop_ticks := 0 minDrvTics := 65535 - drv_state := DS_Unknown + drv_state := DCS_Unknown maxSpeed := 75 ' [1-100] default 75 maxSpeed4dist := 75 ' [1-100] default 75 @@ -153,9 +153,11 @@ PUB start(eMotorBasePin, eMotorVoltage) : ok | legalBase ok := motorCog := coginit(NEWCOG, @driver, @pinbase) + 1 if motorCog == 0 ' did fail? debug("!! ERROR filed to start Motor Control task") + else + debug("* Motor COG #", sdec_(motorCog- 1)) PUB startSenseCog() : ok -'' Start the single motor sense task (tracks position of motor, distance travelled, etc.) +'' Start the single motor sense task (tracks position of motor, distance traveled, etc.) ok := senseCog := cogspin(NEWCOG, taskPostionSense(), @taskStack) + 1 if senseCog == 0 ' did fail? debug("!! ERROR filed to start Position Sense task") @@ -216,6 +218,7 @@ PUB setMaxSpeedForDistance(speed) | limitSpeed4dist PUB calibrate() '' NOT WORKING: (we may need this?) +'' have motor drivers determine fixed-offset constants PUB holdAtStop(bEnable) '' Informs the motor control cog to actively hold position (bEnable=true) or coast (bEnable=false) at end of motion @@ -360,7 +363,7 @@ PUB clearEmergency() debug("-- EMERGENCY -- CLEARED --") PUB forwardIsReverse() -'' call when we have two motors and one of them needs to be reversed +'' Call when we have two motors and one of them needs to be reversed motorIsReversed := TRUE CON { --- Subsystem Status --- } @@ -394,16 +397,16 @@ PUB getDistance(eDistanceUnits) : nDistanceInUnits | fMMpTick, fValue PUB getRotationCount(eRotationUnits) : rotationCount '' Returns accumulated {rotationCount} in {rotationUnits} [DRU_DEGREES, DRU_ROTATIONS, or DRU_HALL_TICKS], since last reset, for this motor. - rotationCount := 0 + rotationCount := -1 case eRotationUnits DRU_HALL_TICKS: - rotationCount := motorStopHallTicks + rotationCount := posTrkHallTicks DRU_DEGREES: ' degrees = ticks * 4 - rotationCount := motorStopHallTicks * 4 + rotationCount := posTrkHallTicks * 4 DRU_ROTATIONS: ' rotations = ticks / 90 - rotationCount := motorStopHallTicks / 90 + rotationCount := posTrkHallTicks / 90 other: debug("! ERROR: invalid rotationUnits Value:", udec_long(eRotationUnits)) abort @@ -437,9 +440,9 @@ PUB getStatus() : eStatus else eStatus := DS_Unknown -PUB getDriveState() : eDrvState +PUB getDriverState() : eDrvrState '' Return Enum [DCS_*] value representing state of driver - eDrvState := drv_state + eDrvrState := drv_state PUB getRawHallTicks() : nPos '' Return the raw driver-maintained tick count @@ -841,10 +844,11 @@ CON { driver interface Constants} ' DCS_SPIN_DN - motor is stopping, then will be at STOPPED ' DCS_SLOWING_DN - motor is slowing down (just a speed change), then will be at AT_SPEED ' DCS_SLOW_TO_CHG - motor is slowing down to change direction, then will be SPEEDING_UP + ' DCS_FAULTED - motor faulted (couldn't drive to requested speed) + ' DCS_ESTOP - motor in emergency stop (external stop requested) ' #0, DCS_Unknown, DCS_STOPPED, DCS_SPIN_UP, DCS_AT_SPEED, DCS_SPIN_DN, DCS_SLOWING_DN, DCS_SLOW_TO_CHG, DCS_FAULTED, DCS_ESTOP - CON { driver internal Constants} FRAME = 1024 * 6 / 2 '6 ADC samples make 1 PWM frame, divide by two for triangle PWM @@ -853,6 +857,9 @@ CON { driver internal Constants} ' OFFSET = 108 ' enforced dead time in clock cycles (at 270_000_000 -> 400 nSec) ' PWMLIM = FRAME - OFFSET 'PWM duty hard limit 1/2 freq of PWM + DRVR_STATUS_LONGS_COUNT = 14 + DRVR_PARAMS_LONGS_COUNT = 13 + VAR { * Data Structure for PASM Driver * } ' remember these are zeroed at run time @@ -867,10 +874,11 @@ VAR { * Data Structure for PASM Driver * } ' / ---------------------------- LONG targetIncre ' 1 long must set to cause motion LONG emerStop ' 1 long T/F where T means float all pins - ' 14 longs come from driver, the 15th, "fault" also from driver but only when happens + ' 14 (DRVR_STATUS_LONGS_COUNT) longs come from driver, the 15th, "fault" also from driver but only when happens LONG drive_u, drive_v, drive_w ' 3 sequential longs returns data continually LONG sense_u, sense_v, sense_w, sense_i ' 4 sequential longs returns data continually - LONG hall, pos, duty, err ' 4 sequential longs returns data continually + LONG hall, pos ' 2 sequential longs returns data continually + LONG duty, err ' 2 sequential longs returns data continually LONG loop_ticks ' 1 sequential long returns control loop duration in ticks continually LONG loop_ctcks ' 1 sequential long returns drive loop duration in ticks continually LONG drv_state ' enum [DCS_STOPPED, DCS_SPIN_UP, DCS_AT_SPEED, DCS_SPIN_DN] @@ -910,7 +918,7 @@ VAR { * Data for Motor Position Tracking * } VAR { Motor Parameters } -' TABLE of 13 parameters the pasm2 driver reads every cycle +' TABLE of 13 (DRVR_PARAMS_LONGS_COUNT) parameters the pasm2 driver reads every cycle LONG offset_fwd ' 96 frac 360 (Doug's motor) LONG offset_rev ' 228 frac 360 (Doug's motor) LONG duty_min ' was 100 << 4 #> OFFSET << 4 but now runtime loaded @@ -930,7 +938,7 @@ DAT { Motor DRIVER } ' ' PASM Driver Program ' - org + org 0 driver rdlong x, ptra++ ' get base pin rdlong params_ptr_, ptra++ ' get parameters pointer @@ -996,7 +1004,7 @@ driver rdlong x, ptra++ ' get base pin mov fwdrev, #0 ' just in case ' do initial read of parms so control loop can use them - setq #13-1 ' load fresh parameter table (13 longs) + setq #DRVR_PARAMS_LONGS_COUNT-1 ' load fresh parameter table (13 DRVR_PARAMS_LONGS_COUNT longs) rdlong params_ptr_+1, params_ptr_ ' preset our starting angle @@ -1394,7 +1402,7 @@ driver rdlong x, ptra++ ' get base pin signx x, #7 ' sign-extend bit7 add pos_, x ' add to current pos_ - setq #13-1 ' load fresh parameter table (13 longs) + setq #DRVR_PARAMS_LONGS_COUNT-1 ' load fresh parameter table (13 DRVR_PARAMS_LONGS_COUNT longs) rdlong params_ptr_+1, params_ptr_ cmpm angle, prior_angle wcz ' if angle has changed, forward or reverse? @@ -1416,7 +1424,7 @@ driver rdlong x, ptra++ ' get base pin cmp x, #125 wc if_nc dirl drive_pins ' at FAULT: float all control pins (make them inputs) if_nc mov drv_state_,#DCS_FAULTED ' also on FAULT: mark our motor as FAULTED - if_nc wrlong #1, ptra[14] ' also on FAULT: report fault + if_nc wrlong #1, ptra[DRVR_STATUS_LONGS_COUNT] ' also on FAULT: report fault sub x, #256/6 wc ' modulate duty if_nc muls x, duty_up_ if_c muls x, duty_dn_ @@ -1428,7 +1436,7 @@ driver rdlong x, ptra++ ' get base pin getct loop_dtcks_ ' how long was this iteration? send to host sub loop_dtcks_, drvrSrtTix - setq #14-1 ' write drive[3]/sense[4]/hall/pos/duty/err/ticks[2]/state to hub + setq #DRVR_STATUS_LONGS_COUNT-1 ' write drive[3]/sense[4]/hall/pos/duty/err/ticks[2]/state to hub wrlong drive_u_, ptra jmp #.loop ' main loop @@ -1553,7 +1561,7 @@ scl_levels res 4 ' ------------------------------ ' PRECISE FORMAT: ' copied in-to driver each loop iteration -params_ptr_ res 1 ' params_ptr must be followed by the 13 parameters +params_ptr_ res 1 ' params_ptr must be followed by the 13 (DRVR_PARAMS_LONGS_COUNT) parameters offset_fwd_ res 1 offset_rev_ res 1 duty_min_ res 1 @@ -1571,7 +1579,7 @@ ramp_slo_ res 1 ' angle decrement: subtract this every 500 uSec ' ------------------------------ ' PRECISE FORMAT: ' copied out-of driver each loop iteration -drive_u_ res 1 ' 14 contiguous longs for return data structure +drive_u_ res 1 ' 14 (DRVR_STATUS_LONGS_COUNT) contiguous longs for return data structure drive_v_ res 1 drive_w_ res 1 sense_u_ res 1 @@ -1587,7 +1595,8 @@ loop_ctcks_ res 1 drv_state_ res 1 ' enum: DCS_STOPPED, DCS_SPIN_UP, DCS_AT_SPEED, DCS_SPIN_DN, etc ' fault is here... (but external only) - fit 496 +' $1f8 for reg cogs + fit 496 ' $1f8(504)? - 470 currently works CON { license } {{ diff --git a/isp_flysky_rx.spin2 b/isp_flysky_rx.spin2 index ef81f73..f629524 100644 --- a/isp_flysky_rx.spin2 +++ b/isp_flysky_rx.spin2 @@ -69,10 +69,11 @@ pub start(desiredRxPin) : cog showSbus() pub stop() -'' stop cogs, release pins + '' Stop cogs, release pins sbus.stop() PUB showDebug(bEnable) + '' TESTING: show, hide debug output if bEnable <> FALSE ' force 0 or -1 bEnable := TRUE if bShowDebug <> bEnable @@ -85,6 +86,7 @@ PUB showDebug(bEnable) bShowDebug := bEnable PUB readChannel(chan) : retValue | validChan + '' Return value of given FlySky Control validChan := 1 #> chan <# MAX_CHANNELS ' legal SBUS Rcvr channel retValue := sbus.read(validChan) ' get value from receiver [0-2047] if bShowDebug @@ -102,6 +104,7 @@ PUB swIsOff(swChan) : bOffState PUB swIsMiddle(swChan) : bMiddleState '' Return T/F where T = switch is in Middle position (only useful for swC) bMiddleState := readSwitch(swChan) == SST_MID + 'debug("- swIsMiddle() = ", sdec_Long_(bMiddleState)) PUB readSwitch(swChan) : eSwState | validSwChan '' Read switch value and decode into switch state (2-pos sw) [if 3-pos divert to other routine] @@ -118,6 +121,7 @@ PUB readSw3Way(swChan) : eSwState | validSwChan, rawValue '' Read 3-pos switch value and decode into switch state validSwChan := CTL_SW_C #> swChan <# CTL_SW_C ' legal SBUS Rxcvr channel rawValue := readChannel(validSwChan) + 'debug("- readSw3Way() = ", udec_(rawValue)) if rawValue < 684 ' bottom 3rd of 0-2047 eSwState := SST_ON else @@ -127,7 +131,7 @@ PUB readSw3Way(swChan) : eSwState | validSwChan, rawValue debug("- readSw3Way(", udec_(validSwChan), ") - ", udec(eSwState)) -pub showSbus() | ch, pos, x, y +PUB showSbus() | ch, pos, x, y '' Show status indication if (sbus.has_signal()) @@ -142,7 +146,7 @@ pub showSbus() | ch, pos, x, y debug("- SBUS Error FRAME LOST") PUB manualCalibrate() -'' prompt and let user set controls to get best readings... + '' Prompt and let user set controls to get best readings... 'term.fstr0(string("- Calibration - flip swA DOWN to start\r\n")) debug("- Calibration - flip swA DOWN to start") repeat diff --git a/isp_host_serial.spin2 b/isp_host_serial.spin2 new file mode 100644 index 0000000..a1e611a --- /dev/null +++ b/isp_host_serial.spin2 @@ -0,0 +1,165 @@ +'' ================================================================================================= +'' +'' File....... isp_host_serial.spin2 +'' Purpose.... (Singleton) Provide host access methods +'' Authors.... Stephen M Moraco +'' -- Copyright (c) 2022 Iron Sheep Productions, LLC +'' -- see below for terms of use +'' E-mail..... stephen@ironsheep.biz +'' Started.... Jan 2022 +'' Updated.... 11 Jan 2022 +'' +'' ================================================================================================= +CON { Object Interface: PUBLIC ENUMs } + + OP_SUCCESS = serialQueue.OP_SUCCESS + OP_FAILURE = serialQueue.OP_FAILURE + + ' tx pin pull-up constants + PU_NONE = serialIF.PU_NONE + PU_1K5 = serialIF.PU_1K5 + PU_3K3 = serialIF.PU_3K3 + PU_15K = serialIF.PU_15K ' pull-up options + + +OBJ { Objects Used by this Object } + + serialIF : "isp_serial_singleton" ' serial I/O + serialQueue : "isp_queue_serial" ' Queued Receive on top of serial I/O + +DAT { pin info saves } + + pinRx long 0 + pinTx long 0 + +DAT { string constants } + + spin2ver byte "1.0.0",0 + + ' ------------------------------- + ' the following are identical to that found in our deamon python script + ' (!!!they must be kept in sync!!!) + parmSep byte "^|^",0 ' chars that will not be found in user data + ' ------------------------------- + +PUB null() +'' This is not a top level object + +PUB startx(rxpin, txpin, baud, pullup) +'' Start serial gateway serial coms on rxpin and txpin at baud + pinRx := rxpin + pinTx := txpin + serialIF.startx(pinRx, pinTx, baud, pullup) ' use user serial port + serialQueue.startx(pinRx, pinTx, baud, pullup) ' start back-end cog + +PUB stop() +'' Release the serial pins (if needed) + serialQueue.stop() ' stop back-end cog + serialIF.rxflush() ' git rid of any pending input + ' free the pins used + pinf(pinRx) ' de-assert + pinf(pinTx) ' de-assert + +PUB identify(pHardwareId) +'' Report to the serial host the name of this device (and object version) + serialIF.fstr3(string("ident:hwName=%s%sobjVer=%s\n"), pHardwareId, @parmSep, @spin2ver) + ' parse expected result display error (via debug) if occurred + handleResponseFor(string("fident")) + + +PUB getErrorCtrs() : nRPiCt, nP2Ct +'' Return the test Tx and Rx error counters (cleared on sendTestMessage(reset=true)) +nRPiCt := testTxErrCt +nP2Ct := testRxErrCt + +DAT { ----------- PRIVATE Serial Test support ----------- } + + testTxCount long 0 + testRxCount long 0 + + testTxErrCt long 0 + testRxErrCt long 0 + + valTrueStr byte "True",0 + valFalseStr byte "False",0 + + testTxMsg byte "P2TestMsg#" + testTxMsgCt byte "00000",0 + + testRxMsg byte "RPiTestMsg#" + testRxMsgCt byte "00000",0 + +PRI genRxTestMessage(nCountValue) : pStr | nThous, nHun, nTens, nOnes, nRemainder +' fill-in expected Rx Message with count + placeMessageCount(nCountValue, @testRxMsgCt) + pStr := @testRxMsg + +PRI genTestMessage(nCountValue) : pStr | nThous, nHun, nTens, nOnes, nRemainder +' fill-in next Tx Message with count + placeMessageCount(nCountValue, @testTxMsgCt) + pStr := @testTxMsg + +PRI placeMessageCount(nCountValue, pCountOffset) | nThous, nHun, nTens, nOnes, nRemainder +' place digits representing count into our message + bytefill(pCountOffset, $30, 5) ' preload w/ASCII ZEROs + nThous := nCountValue / 1000 + nRemainder := nCountValue - (nThous * 1000) + nHun := nRemainder / 100 + nRemainder := nRemainder - (nHun * 100) + nTens := nRemainder / 10 + nOnes := nRemainder - (nTens * 10) + byte[pCountOffset][1] := nThous + $30 + byte[pCountOffset][2] := nHun + $30 + byte[pCountOffset][3] := nTens + $30 + byte[pCountOffset][4] := nOnes + $30 + +CON { ----------- PRIVATE Methods ----------- } + + MAX_LEN_USERMSG = 128 + WAIT_FOR_STRING = TRUE + +DAT + userMsgBuffer BYTE 0[MAX_LEN_USERMSG+1] + +PRI handleResponseFor(pStr) + serialQueue.getLine(@userMsgBuffer, MAX_LEN_USERMSG, WAIT_FOR_STRING) + if not strHasPrefix(@userMsgBuffer, pStr) + debug("hrf: unexpected response!") + debug("hrf: ERROR: [", zstr_(pStr), "] NOT found in [", zstr_(@userMsgBuffer), "]") + else + debug("hrf: RX str(", udec_(strsize(pStr)), ")=[", zstr_(pStr), "]") + +PRI strHasPrefix(pTargetStr, pPrefix) : bHasStatus | nIdx + bHasStatus := TRUE + repeat nIdx from 0 to strsize(pPrefix) - 1 + if BYTE[pTargetStr][nIdx] <> BYTE[pPrefix][nIdx] + bHasStatus := False + quit ' outta here, we have our answer! + +CON { license } +{{ + + ------------------------------------------------------------------------------------------------- + MIT License + + Copyright (c) 2022 Iron Sheep Productions, LLC + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + ================================================================================================= +}} diff --git a/isp_mem_strings.spin2 b/isp_mem_strings.spin2 new file mode 100644 index 0000000..ea1926b --- /dev/null +++ b/isp_mem_strings.spin2 @@ -0,0 +1,271 @@ +'' ================================================================================================= +'' +'' File....... isp_mem_strings.spin2 +'' Purpose.... provide sprintf() c-like functions for Spin2 developers +'' Authors.... Stephen M. Moraco +'' (Highly leveraged from jm_serial.spin2 by Jon McPhalen) +'' E-mail..... stephen@ironsheep.biz +'' Started.... Feb 2022 +'' Updated.... 4 Feb 2022 +'' +'' ================================================================================================= + +OBJ { Objects Used by this Object } + + nstr : "jm_nstr" ' number-to-string + +PUB null() + + '' This is not a top level object + +CON { pst formatting } + + HOME = 1 + CRSR_XY = 2 + CRSR_LF = 3 + CRSR_RT = 4 + CRSR_UP = 5 + CRSR_DN = 6 + BELL = 7 + BKSP = 8 + TAB = 9 + LF = 10 + CLR_EOL = 11 + CLR_DN = 12 + CR = 13 + CRSR_X = 14 + CRSR_Y = 15 + CLS = 16 + +CON { formatted strings } + +{{ + + ------------------------------------------------------------------------------------------------- + Escaped characters + + \\ backslash char + \% percent char + \q double quote + \b backspace + \t tab (horizontal) + \n new line (vertical tab) + \r carriage return + \nnn arbitrary ASCII value (nnn is decimal) + + Formatted arguments + + %w.pf print argument as decimal width decimal point + %[w[.p]]d print argument as decimal + %[w[.p]]x print argument as hex + %[w[.p]]o print argument as octal + %[w[.p]]q print argument as quarternary + %[w[.p]]b print argument as binary + %[w]s print argument as string + %[w]c print argument as character ( + + -- w is field width + * positive w causes right alignment in field + * negative w causes left alignment in field + -- %ws aligns s in field (may truncate) + -- %wc prints w copies of c + -- p is precision characters + * number of characters to use, aligned in field + -- prepends 0 if needed to match p + -- for %w.pf, p is number of digits after decimal point + ------------------------------------------------------------------------------------------------- + +}} + +PUB sFormatStr0(pUserBuff, p_str) : nPlaced +'' Format string w/o-args into {pUserBuff} with formatting characters, zero terminated +'' Returns count of characters placed into the users buffer (excl zero term.) + nPlaced := sFormat(pUserBuff, p_str, 0) + + +PUB sFormatStr1(pUserBuff, p_str, arg1): nPlaced +'' Format string and 1-arg into {pUserBuff} with formatting characters, zero terminated +'' Returns count of characters placed into the users buffer (excl zero term.) + nPlaced := sFormat(pUserBuff, p_str, @arg1) + +PUB sFormatStr2(pUserBuff, p_str, arg1, arg2): nPlaced +'' Format string and 2-args into {pUserBuff} with formatting characters, zero terminated +'' Returns count of characters placed into the users buffer (excl zero term.) + nPlaced := sFormat(pUserBuff, p_str, @arg1) + +PUB sFormatStr3(pUserBuff, p_str, arg1, arg2, arg3): nPlaced +'' Format string and 3-args into {pUserBuff} with formatting characters, zero terminated +'' Returns count of characters placed into the users buffer (excl zero term.) + nPlaced := sFormat(pUserBuff, p_str, @arg1) + +PUB sFormatStr4(pUserBuff, p_str, arg1, arg2, arg3, arg4): nPlaced +'' Format string and 4-args into {pUserBuff} with formatting characters, zero terminated +'' Returns count of characters placed into the users buffer (excl zero term.) + nPlaced := sFormat(pUserBuff, p_str, @arg1) + +PUB sFormatStr5(pUserBuff, p_str, arg1, arg2, arg3, arg4, arg5): nPlaced +'' Format string and 5-args into {pUserBuff} with formatting characters, zero terminated +'' Returns count of characters placed into the users buffer (excl zero term.) + nPlaced := sFormat(pUserBuff, p_str, @arg1) + +PUB sFormatStr6(pUserBuff, p_str, arg1, arg2, arg3, arg4, arg5, arg6): nPlaced +'' Format string and 6-args into {pUserBuff} with formatting characters, zero terminated +'' Returns count of characters placed into the users buffer (excl zero term.) + nPlaced := sFormat(pUserBuff, p_str, @arg1) + +PUB sFormatStr7(pUserBuff, p_str, arg1, arg2, arg3, arg4, arg5, arg6, arg7): nPlaced +'' Format string and 7-args into {pUserBuff} with formatting characters, zero terminated +'' Returns count of characters placed into the users buffer (excl zero term.) + nPlaced := sFormat(pUserBuff, p_str, @arg1) + +PUB sFormatStr8(pUserBuff, p_str, arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8): nPlaced +'' Format string and 8-args into {pUserBuff} with formatting characters, zero terminated +'' Returns count of characters placed into the users buffer (excl zero term.) + nPlaced := sFormat(pUserBuff, p_str, @arg1) + + +PUB sFormat(pUserBuff, p_str, p_args) : nPlaced | idx, c, asc, field, digits, pBffr +'' Format string with escape sequences and embedded values to {pOutBuf} +'' -- {pUserBuff} is a pointer to the users buffer to receive the formatted string +'' -- {p_str} is a pointer to the format control string +'' -- {p_args} is pointer to array of longs that hold field values +'' * field values can be numbers, characters, or pointers to strings + 'debug("sfmt: fstr(", udec_(strsize(p_str)), ")=[", zstr_(p_str), "]") + + idx := 0 ' value index + pBffr := pUserBuff + repeat + c := byte[p_str++] + if (c == 0) + quit + + elseif (c == "\") + c := byte[p_str++] + if (c == "\") + sTx("\", pBffr++) + elseif (c == "%") + sTx("%", pBffr++) + elseif (c == "q") + sTx(34, pBffr++) + elseif (c == "b") + sTx(BKSP, pBffr++) + elseif (c == "t") + sTx(TAB, pBffr++) + elseif (c == "n") + sTx(LF, pBffr++) + elseif (c == "r") + sTx(CR, pBffr++) + elseif ((c >= "0") and (c <= "9")) + --p_str + p_str, asc, _ := get_nargs(p_str) + if ((asc >= 0) and (asc <= 255)) + sTx(asc, pBffr++) + + elseif (c == "%") + p_str, field, digits := get_nargs(p_str) + c := byte[p_str++] + if (c == "f") + pBffr := sStr(nstr.fmt_number(long[p_args][idx++], 99, digits, field, " "), pBffr) + elseif (c == "d") + pBffr := sStr(nstr.fmt_number(long[p_args][idx++], 10, digits, field, " "), pBffr) + elseif (c == "x") + pBffr := sStr(nstr.fmt_number(long[p_args][idx++], 16, digits, field, " "), pBffr) + elseif (c == "o") + pBffr := sStr(nstr.fmt_number(long[p_args][idx++], 08, digits, field, " "), pBffr) + elseif (c == "q") + pBffr := sStr(nstr.fmt_number(long[p_args][idx++], 04, digits, field, " "), pBffr) + elseif (c == "b") + pBffr := sStr(nstr.fmt_number(long[p_args][idx++], 02, digits, field, " "), pBffr) + elseif (c == "s") + pBffr := sStr(nstr.padstr(long[p_args][idx++], field, " "), pBffr) + elseif (c == "c") + pBffr := sTxn(long[p_args][idx++], (abs field) #> 1, pBffr) + + else + sTx(c, pBffr++) + + byte[pBffr] := 0 ' place terminator + nPlaced := strsize(pUserBuff) + 'debug("sfmt: ", UHEX_BYTE_ARRAY_(pUserBuff, 48)) + debug("sfmt: str(", udec_(nPlaced), ")=[", zstr_(pUserBuff), "]") + +CON { --- Internal Methods --- } + +PRI sTx(nChr, pOutBuf) +' write char to bfr + byte[pOutBuf] := nChr + +PRI sTxn(nChr, nCount, pOutBuf) : pBufNext +' Emit byte n times + repeat nCount + sTx(nChr, pOutBuf++) + + pBufNext := pOutBuf + +PRI sStr(pStr, pOutBuf) : pBufNext +' write char to bfr + repeat (strsize(pStr)) + sTx(byte[pStr++], pOutBuf++) + + pBufNext := pOutBuf + +PRI get_nargs(p_str) : p_str1, val1, val2 | c, sign +' Parse one or two numbers from string in n, -n, n.n, or -n.n format +' -- dpoint separates values +' -- only first # may be negative +' -- returns pointer to 1st char after value(s) + + c := byte[p_str] ' check for negative on first value + if (c == "-") + sign := -1 + ++p_str + else + sign := 0 + + repeat ' get first value + c := byte[p_str++] + if ((c >= "0") and (c <= "9")) + val1 := (val1 * 10) + (c - "0") + else + if (sign) + val1 := -val1 + quit + + if (c == ".") ' if dpoint + repeat ' get second value + c := byte[p_str++] + if ((c >= "0") and (c <= "9")) + val2 := (val2 * 10) + (c - "0") + else + quit + + p_str1 := p_str - 1 ' back up to non-digit + + +CON { license } +{{ + + ------------------------------------------------------------------------------------------------- + MIT License + + Copyright (c) 2022 Iron Sheep Productions, LLC + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + ================================================================================================= +}} diff --git a/isp_queue_serial.spin2 b/isp_queue_serial.spin2 new file mode 100644 index 0000000..d7f1d77 --- /dev/null +++ b/isp_queue_serial.spin2 @@ -0,0 +1,883 @@ +'' ================================================================================================= +'' +'' File....... isp_queue_serial.spin2 +'' Purpose.... provide serial line receiver that queues each line received +'' +'' Authors.... Stephen M. Moraco +'' -- see below for terms of use +'' E-mail..... stephen@ironsheep.biz +'' Started.... Jan 2022 +'' Updated.... 04 May 2022 +'' +'' ================================================================================================= + +' implemented as a receive COG storing characters in a larger circular buffer. As complete strings +' are identified (denoted by LF char at end) the LF is replaced by a zero terminator and the +' pointer to the newly arrived string is stored in a circular queue of arrived strings. + + +CON { Object Interface: PUBLIC ENUMs } + + #200, FM_READONLY, FM_WRITE, FM_WRITE_CREATE, FM_LISTEN + #0, OP_SUCCESS, OP_FAILURE + +#0, ENT_VALUE_CT_IDX, ENT_RAWSTR_IDX, ENT_CMDSTR_IDX, ENT_CMDENUM_IDX, ENT_PARM1_IDX, ENT_PARM2_IDX, ENT_PARM3_IDX, ENT_PARM4_IDX + +CON { fixed io pins } + + RX1 = 63 { I } ' programming / debug + TX1 = 62 { O } + + SF_CS = 61 { O } ' serial flash + SF_SCK = 60 { O } + SF_SDO = 59 { O } + SF_SDI = 58 { I } + +OBJ { Objects Used by this Object } + + serialChan : "isp_serial_singleton" ' serial I/O + +CON { driver config values } + + STACK_SIZE_LONGS = 64 ' 48, 32 crashes! + + RX_CHR_Q_MAX_BYTES = 512 ' 80 = testing size, else 512 + MAX_SINGLE_STRING_LEN = 128 ' 79 = testing size, else 128 + RX_STR_Q_MAX_LONGS = 10 ' 10 strings waiting to be picked up - MAX + + RX_BFFR_LEN = 128 + RX_COMMAND_MAX_LEN = 32 + RX_VALUE_MAX_LEN = 32 + RX_MAX_NAMES = 20 + + MAX_COMMAND_PARTS = 10 ' count, raw string, cmdStr, eCmd, 6 paramValues + +CON { test values } +{ + CLK_FREQ = 270_000_000 ' system freq as a constant + _clkfreq = CLK_FREQ ' set system clock + + RX_GW = 25 '- I - ' Exxternal Host (RPi/Arduino) Gateway + TX_GW = 24 '- O - + + GW_BAUDRATE = 624_000 ' 624kb/s - allow P2 rx to keep up! +} + DO_NOT_WRITE_MARK = $addee5e5 + NOT_WRITTEN_MARK = $a5a50df0 + +VAR { pin info saves } + + long pinRx + long pinTx + + long rxCogId + + +DAT { Queued RX data } + +' user set variables at task init +pRxByteStart long 0 +nRxByteMax long 0 + +bTestMode long FALSE + +' TASK use variables +taskStack long 0[STACK_SIZE_LONGS] +endStackMark long DO_NOT_WRITE_MARK + +pRxByteHead long 0 ' our QUEUE control vars - received BYTEs +nRxByteCount long 0 + +bInString long FALSE +pNewStrStart long 0 + +pRsltStrStart long 0 +pRsltQueOverrun long FALSE + +bQueOverrun long FALSE +bStrQueOverrun long FALSE + +nQStrHeadIdx long 0 ' our QUEUE control vars - received strings +nQStrTailIdx long 0 +nQStrCount long 0 +nQStrFill long $deadf00d +rxStrPtrSet long 0[RX_STR_Q_MAX_LONGS] ' our circular buffer - ptrs to received strings + +serialRxBffr byte 0[RX_CHR_Q_MAX_BYTES] ' our circular buffer - received chars + +' task vars +nLoopCt long 0 +pNextMsg long 0 +nTaskChr long 0 +' tsk vars + + ' pValues -> [0:nbrValues(1-n)] (doesn't count "full string") ENT_VALUE_CT_IDX + ' [1:pStr] -> "full string" ENT_RAWSTR_IDX + ' [2:pCmd] -> "cmd" ENT_CMDSTR_IDX + ' [3:eCmd] ENT_CMDENUM_IDX + ' [4:nValue1] ENT_PARM1_IDX + ' [5:nValue2] ENT_PARM2_IDX + ' [6:nValue3] ENT_PARM3_IDX + ' [7:nValue4] ENT_PARM4_IDX + ' [etc] - up to 6 max values + +currParsedCommand LONG 0[MAX_COMMAND_PARTS] + +currSingleLineBffr byte 0[MAX_SINGLE_STRING_LEN+1] +currCommandName byte 0[RX_COMMAND_MAX_LEN+1] + +tmpValueStr byte 0[RX_VALUE_MAX_LEN+1] + + +CON { - - - - - } + +CON { test control values } + + #0, CT_UNKNOWN, CT_IDLE, CT_STOP, CT_PLACE_STR, CT_PLACE_N_MSGS + + #0, TST_UNKNOWN, TST_PASS, TST_FAIL + +DAT { test control vars } + +bDbgShoMem long FALSE + +eTestCtrl long 0 +eTestParam long 0 +eTestCtrlPrior long 0 + +ctStrIdle byte "IDLE",0 +ctStrStop byte "STOP",0 +ctStrPlcStr byte "PLACE_STR",0 +ctStrPlcNMsgs byte "PLACE_N_MSGS",0 +ctStrUnknown byte "CT_????",0 + +greenStr byte "Green String", $0a, 0 +redStr byte "Red String", $0a, 0 + +nStackDepth long STACK_SIZE_LONGS + +PUB null() | pWrappedStr, bStatus, pStr, bWaitStatus, nCtParm, bPassFail +'' This is not a top level object + +CON { - - - - - } + +PUB startx(rxpin, txpin, baud, pullup) +'' Start serial gateway serial coms on rxpin and txpin at baud + pinRx := rxpin + pinTx := txpin + serialChan.startx(pinRx, pinTx, baud, pullup) ' use user serial port + + prepStackForCheck() ' for our internal test use + + ' start our rx task in own cog + rxCogId := cogspin(newcog, TaskSerialRx(@serialRxBffr, RX_CHR_Q_MAX_BYTES), @taskStack) + if rxCogId == -1 ' did fail? + debug("!! ERROR filed to start RX-QUE task") + +PUB stop() +'' Release the serial pins and free up the rcvr cog + serialChan.rxflush() ' git rid of any pending input + ' free the pins used + pinf(pinRx) ' de-assert + pinf(pinTx) ' de-assert + + ' free the cog used + if(rxCogId) + cogstop(rxCogId - 1) + rxCogId := 0 + +CON { ----- Control Changes Interface ----- } + + WAIT_FOR_STRING = TRUE + + ' offsets for incoming table entries + #0, TBLIDX_POSSCMD, TBLIDX_ECMD, TBLIDX_PARAMCT + +PUB haveCommand(pEntries, nEntryCount) : bHaveCmdStatus | opStatus, pValues, nIdx, nParamCt, pCmd, eCmd, bGoodCmd +'' Return T/F where T means a control value change has been received and needs to be handled + bHaveCmdStatus := haveRxString() + if nQStrCount <> 0 + debug("- hvcmd: ", uhex_long(pEntries), udec_long(nEntryCount), udec_long(nQStrCount)) + 'repeat ' lock here + + ' validation table: (nEntryCount) entries where each is: + ' TBLIDX_POSSCMD, TBLIDX_ECMD, TBLIDX_PARAMCT + ' [pCmdStr][eCmdValue][nParamCt] + ' [pCmdStr][eCmdValue][nParamCt] + ' : + ' : + ' [pCmdStr][eCmdValue][nParamCt] + + ' Interpret string, do partial validation + ' if NOT valid, send ERROR w/cause over serial and say we have no string + ' else return OK and say we have request to be handled + if (bHaveCmdStatus == TRUE) + bHaveCmdStatus := FALSE ' preset as invalid command... + ' debug("!! message arrived") + opStatus, pValues := parseCommandAndValue() + if opStatus <> OP_SUCCESS + sendError(@"Unrecognized String") + else + ' pValues -> [0:nbrValues(1-n)] (doesn't count "full string") ENT_VALUE_CT_IDX + ' [1:pStr] -> "full string" ENT_RAWSTR_IDX + ' [2:pCmd] -> "cmd" ENT_CMDSTR_IDX + ' [3:eCmd] ENT_CMDENUM_IDX + ' [4:nValue1] ENT_PARM1_IDX + ' [5:nValue2] ENT_PARM2_IDX + ' [6:nValue3] ENT_PARM3_IDX + ' [7:nValue4] ENT_PARM4_IDX + ' [etc] - up to 6 max values + + ' locate command in table + bGoodCmd := FALSE + repeat nIdx from 0 to nEntryCount - 1 + pCmd := LONG [pEntries][(nIdx * 3) + TBLIDX_POSSCMD] + eCmd := LONG [pEntries][(nIdx * 3) + TBLIDX_ECMD] + nParamCt := LONG [pEntries][(nIdx * 3) + TBLIDX_PARAMCT] + 'debug("- hvcmd: tblent #", udec_(nIdx), " [", zstr_(pCmd), "] eCmd=", sdec_(eCmd)) + ' is this our cmd? + if strIgnoreCaseMatch(pCmd, LONG[pValues][ENT_CMDSTR_IDX]) + bGoodCmd := TRUE + quit + ' if not found ERROR unknown command, return NO command + if not bGoodCmd + sendError(@"Command NOT found") + else + ' record enum for found command + LONG[pValues][ENT_CMDENUM_IDX] := eCmd + ' check nbr params (in table) + ' if not match ERROR bad nbr params, return NO command + if nParamCt <> LONG[pValues][ENT_VALUE_CT_IDX] - 1 + sendError(@"Missing/Extra parameter(s)") + else + ' return valid-so-far command + bHaveCmdStatus := TRUE + dumpParsedCommand(@"hvCmd") + +PUB sendError(pMsg) +'' Reply to sender with validation error message + serialChan.str(@"ERROR ") + serialChan.str(pMsg) + serialChan.str(@"\n") + debug(" ERROR: [", zstr_(pMsg), "] raw=[", zstr_(@currSingleLineBffr), "]") + +PUB sendOK() +'' Reply to sender with OK message + serialChan.str(@"OK\n") + debug(" OK: raw=[", zstr_(@currSingleLineBffr), "]") + +PUB sendResponse(pMsg) +'' Send requested data back to sender + serialChan.str(pMsg) + serialChan.str(@"\n") + debug(" response: ", zstr_(pMsg)) + +PRI parseCommandAndValue() : opStatus, pValues | pStr +' Interpret command and return descriptor + debug("- parseCommandAndValue()") +' where descriptor is: +' pValues -> [0:nbrValues(1-n)] (doesn't count "full string") ENT_VALUE_CT_IDX +' [1:pStr] -> "full string" ENT_RAWSTR_IDX +' [2:pCmd] -> "cmd" ENT_CMDSTR_IDX +' [3:eCmd] ENT_CMDENUM_IDX +' [4:nValue1] ENT_PARM1_IDX +' [5:nValue2] ENT_PARM2_IDX +' [6:nValue3] ENT_PARM3_IDX +' [7:nValue4] ENT_PARM4_IDX +' [etc] - up to 6 max values + pStr := getLine(@currSingleLineBffr, MAX_SINGLE_STRING_LEN, WAIT_FOR_STRING) + ' now split string into useful parts + opStatus, pValues := commandParse(pStr) + +PUB getCommandParms() : opStatus, pValues +'' Return latest descriptor to caller + debug("- getCommandParms()") +' where descriptor is: +' pValues -> [0:nbrValues(1-n)] (doesn't count "full string") ENT_VALUE_CT_IDX +' [1:pStr] -> "full string" ENT_RAWSTR_IDX +' [2:pCmd] -> "cmd" ENT_CMDSTR_IDX +' [3:eCmd] ENT_CMDENUM_IDX +' [4:nValue1] ENT_PARM1_IDX +' [5:nValue2] ENT_PARM2_IDX +' [6:nValue3] ENT_PARM3_IDX +' [7:nValue4] ENT_PARM4_IDX +' [etc] - up to 6 max values + + ' if we have values in parsed table () (PROXY: is command was recognized) + opStatus := (LONG[@currParsedCommand][ENT_CMDENUM_IDX] > 0) ? OP_SUCCESS : OP_FAILURE + pValues := @currParsedCommand + +CON { ----- General Strings Interface ----- } + +PUB haveRxString() : bPresentStatus +'' Return T/F where T means a control value change has been received and needs to be handled + bPresentStatus := (nQStrCount > 0) ? TRUE : FALSE + 'debug("?? haveRxString[T/F]: ", ubin_byte(presentStatus)) + +PUB rxStringsCount() : nCount +'' Return count of strings received + nCount := nQStrCount + +PUB getLine(pUserDest, lenDest, bShouldWait) : pStr | bStringArrived, pWrappedStr, nLen +'' Return {pStr} or 0 if none +'' if {bShouldWait} is TRUE wait until string arrives before returning + 'debug("getLine: ENTRY") + pStr := 0 + if bShouldWait == TRUE + bStringArrived := TRUE + repeat until haveRxString() == TRUE + else + bStringArrived := haveRxString() + + if bStringArrived == TRUE + pStr := pUserDest + pWrappedStr := dequeueRxStr() + bytefill(pUserDest, 0, lenDest) + copyWrappedStr(pUserDest, pWrappedStr, lenDest) + nLen := strsize(pUserDest) + freeWrappedString(pWrappedStr, nLen) + 'debug("gcs: str=[", zstr_(pUserDest), "]") + 'debug("- gtln: str(", udec_(nLen), ")=[", zstr_(pUserDest), "]") + else + debug("gtln: ERROR str=[] - NOT PRESENT") + + +PUB flushRxQueue() +'' Reset all Rx Queue control and data (emptying it!) + bytefill(@serialRxBffr, 0, RX_CHR_Q_MAX_BYTES) + longfill(@rxStrPtrSet, 0, RX_STR_Q_MAX_LONGS) + pRxByteHead := pRxByteStart + + bInString := FALSE + + nQStrHeadIdx := 0 + nQStrTailIdx := 0 + + nRxByteCount := 0 + nQStrCount := 0 + + pRsltStrStart := 0 + +PUB resetRxStatus() +'' Reset all task failure indicators + bQueOverrun := FALSE + bStrQueOverrun := FALSE + +PUB decimalForString(pDecimalStr) : decimalValue | nCharCt, nIdx, bHaveNeg +'' Return long value for given decimal string + decimalValue := 0 + nCharCt := strsize(pDecimalStr) + if (nCharCt > 0) + bHaveNeg := (BYTE[pDecimalStr][0] == "-") ? TRUE : FALSE + if bHaveNeg + pDecimalStr++ ' point past neg sign + nCharCt-- + if (nCharCt > 0) + repeat nIdx from 0 to nCharCt - 1 + decimalValue := (decimalValue * 10) + (BYTE[pDecimalStr][nIdx] - $30) + if bHaveNeg + decimalValue := 0 - decimalValue + +PUB hexadecimalForString(pHexStr) : valueOfHex | nChar, nIdx, nOffset, nDigitValue +'' Return long value for given hexadecimal string + nOffset := 0 + 'debug("CONV: [", zstr_(pHexStr), "]") + if(strHasLowCasePrefix(pHexStr, string("0x"))) ' handle both 0xFEA94 and FEA94 + ' skip over prefix + nOffset := 2 + valueOfHex := 0 + repeat nIdx from nOffset to strsize(pHexStr) - 1 + valueOfHex *= 16 + nChar := BYTE[pHexStr][nIdx] & $DF ' convert upper case to lower + if nchar >= $30 && nchar <= $46 + if nChar > $39 + nDigitValue := nChar - $41 + 10 ' remove 'A' but leave as 10-15 + else + nDigitValue := nChar - $30 ' remove '0' leave as 0-9 + valueOfHex += nDigitValue + 'debug("CONV: ", uhex(nIdx), ", ",uhex(nChar), ", ", uhex(nDigitValue), ", ", uhex(valueOfHex)) + + 'debug("CONV: [", zstr_(pHexStr), "]= ", uhex_long(valueOfHex)) + +CON { ----- TASK ----- } +' ==================================================== +' SERIAL RX QUEUE routines +' +PRI TaskSerialRx(pRxBffr, lenRxBffr) +' our serial receive to queue loop + ' preserve incoming values + pRxByteStart := pRxBffr + nRxByteMax := lenRxBffr + + ' NOTE parameters are for DEBUG use only + flushRxQueue() + + debug("TASK[Rx] started ", uhex(pRxByteStart), ", ", udec(nRxByteMax), ", rxBffr=[", uhex_(@serialRxBffr), "]") + + if bTestMode + ' run loop (test-version for now) + debug("TASK[Rx] ** TEST LOOP Running **") + repeat + checkStack() + if (eTestCtrl == CT_PLACE_STR) + eTestCtrl := CT_IDLE + tskFakeRxStr(eTestParam) + elseif (eTestCtrl == CT_PLACE_N_MSGS) + eTestCtrl := CT_IDLE + nLoopCt := eTestParam + repeat nLoopCt + pNextMsg := genTestMsg() + tskFakeRxStr(pNextMsg) + if (eTestCtrlPrior <> eTestCtrl) + showTestState(eTestCtrlPrior, eTestCtrl) + eTestCtrlPrior := eTestCtrl + else + ' read serial input forever placing chars in RX Circ Queue + debug("TASK[Rx] ** Live LOOP rcvg fm serial **") + repeat + nTaskChr := serialChan.rxtime(1000) + if (nTaskChr <> -1) + tskEnqueueChar(nTaskChr) + +PRI tskFakeRxStr(pStr) | nIdx, nStatusValue +' place string into buffer as if it was received + debug("TASK[Rx] str=[", zstr_(pStr), "]") + repeat nIdx from 0 to strsize(pStr) - 1 + nStatusValue := tskEnqueueChar(byte[pStr][nIdx]) + if nStatusValue + debug("EEE Abort string write EEE") + quit + +PRI tskEnqueueChar(nChr) : bFailedStatus +' place byte into rx queue, if EOL then place term instead and engueue string ptr! + bFailedStatus := FALSE + if (nRxByteCount < nRxByteMax) + if bInString == FALSE + bInString := TRUE + pNewStrStart := pRxByteHead + 'debug("TASK[Rx] ", uhex_long(pNewStrStart), ubin_byte(bInString)) + 'debug("TASK[Rx] rxChr=", uhex_(nChr)) + nRxByteCount++ + if (nChr <> $0a) + ' if NOT LF then save it + byte [pRxByteHead++] := nChr + else + ' have LF terminate line + byte [pRxByteHead++] := $00 ' place term instead of EOL + if bInString == TRUE + bInString := FALSE + 'debug("TASK[Rx] STR=[", zstr_(pNewStrStart), "]") + tskEnqueueStr(pNewStrStart) + ' wrap ptr if needed... + if pRxByteHead > @BYTE [@serialRxBffr][RX_CHR_Q_MAX_BYTES - 1] + pRxByteHead := @serialRxBffr + else + bQueOverrun := TRUE ' signal that we lost incoming data!!! + debug("TASK[Rx] !! ERROR char-queue full!! ", sdec(nRxByteCount), sdec(nRxByteMax)) + bFailedStatus := TRUE + + +PRI tskEnqueueStr(pStr) | strIdx +' report string arrival to listener (place string pointer in queue) + checkGoodStringPtr(pStr) + if nQStrCount < RX_STR_Q_MAX_LONGS + strIdx := nQStrHeadIdx ' save for debug + LONG [@rxStrPtrSet][nQStrHeadIdx++] := pStr + ' if head goes off end-of-set then wrap + if nQStrHeadIdx > RX_STR_Q_MAX_LONGS - 1 + nQStrHeadIdx := 0 + ' mark arrival of new in queue + nQStrCount++ + ' report new string arrival + debug("TASK[Rx] newStr: ", uhex_long_(pStr), " #(", udec_(strIdx), ")") + else + bStrQueOverrun := TRUE ' signal that we lost incoming data!!! + debug("TASK[Rx] !! ERROR ctrl-Q full!!") + 'debug("TASK[Rx] enqueueStr: ", udec(nQStrCount)) + +PRI checkGoodStringPtr(pStr) | pStrLastByte +' report and halt if string not valid (with circular queue range) +' NOTE: string can wrap in circ-que so we don't check end of string... + if pStr < @serialRxBffr or pStr > @BYTE[@serialRxBffr][RX_CHR_Q_MAX_BYTES-1] + debug("EEE have BAD ptr to string! ", uhex_long(pStr), uhex_long(pRxByteStart), uhex_long(pRxByteStart + RX_CHR_Q_MAX_BYTES - 1)) + lockupForAnly() + +PRI lockupForAnly() +' tell that we are halting then halt (effictivly) + debug("---- FAIL - stopping here for Analysis of above ^^^") + repeat ' hold here for now + +CON { ----- Support ----- } + +PRI dequeueRxStr() : pRmStr +' remove string from queue, listener done with it + 'dumpStrQ(string("B4 String Ptr Que")) + if nQStrCount > 0 + pRmStr := LONG[@rxStrPtrSet][nQStrTailIdx++] + if nQStrTailIdx > RX_STR_Q_MAX_LONGS - 1 + nQStrTailIdx := 0 + nQStrCount-- + if bDbgShoMem + debug("-- dqrs:", uhex_long(pRmStr), udec(nQStrCount)) + else + debug("-- dequeueRxStr() !! ERROR string-queue empty!!") + 'dumpStrQ(string("FTER String Ptr Que")) + +PRI dequeueResultRxStr() : pRsltStr +' remove string from queue, listener done with it + 'dumpStrQ(string("B4 String Ptr Que")) + pRsltStr := 0 + if pRsltStrStart <> 0 + pRsltStr := pRsltStrStart + pRsltStrStart := 0 ' mark as empty + +PRI freeWrappedString(pRmStr, nLen) + ' zero our string memory + if bDbgShoMem + debug("-- fws:", uhex_long(pRmStr)) + zeroWrappedStr(pRmStr, nLen) + +PRI zeroWrappedStr(pRmStr, nLen) | nIdx, pSrc +' fill space occuppied by string with zero's +' NOTE handle buffer wrap! +' string can start near and and wrap to front! + pSrc := pRmStr + repeat nIdx from 0 to nLen - 1 + ' if pointing beyond end, wrap to front! + if pSrc > @BYTE[@serialRxBffr][RX_CHR_Q_MAX_BYTES-1] + pSrc -= RX_CHR_Q_MAX_BYTES + BYTE[pSrc++] := 0 + nRxByteCount -= nLen + 1 + if bDbgShoMem + debug("-- zws: ", uhex_long(pRmStr), udec(nLen), udec(nRxByteCount)) + +PRI copyWrappedStr(pUserDest, pSrcStr, lenDest) | nIdx, pSrc, pDest +' copy possible wrapped string {pSrcStr} to {pUserDest} (use min(strlen,lenDest) as bytes to move) +' NOTE handle buffer wrap! +' string can start near and and wrap to front! + pDest := pUserDest + pSrc:= pSrcStr + repeat nIdx from 0 to lenDest - 1 + ' if pointing beyond end, wrap to front! + if pSrc > @BYTE[@serialRxBffr][RX_CHR_Q_MAX_BYTES-1] + pSrc -= RX_CHR_Q_MAX_BYTES + if BYTE[pSrc] == 0 + quit ' at string end, quit loop + BYTE[pDest++] := BYTE[pSrc++] + BYTE[pDest] := 0 ' place final terminator + if bDbgShoMem + debug("-- cws: str(", udec_(strsize(pSrcStr)), ")=[", zstr_(pUserDest), "]") + +PRI commandParse(pRxLine) : nStatus, pValues | nLenRemaining, nSepIdx, pSrchBffr, pRemainingBffr, nCmdLen, nParmIdx + ' Examples: "drivedir {pwr} {dir}\n" + ' Examples: "drivedist {ltdist} {rtdist} {d-u}\n" + ' Examples: "emercutoff\n" + + ' BUILD THIS: + ' pValues -> [0:nbrValues(1-n)] (doesn't count "full string") ENT_VALUE_CT_IDX + ' [1:pStr] -> "full string" ENT_RAWSTR_IDX + ' [2:pCmd] -> "cmd" ENT_CMDSTR_IDX + ' [3:eCmd] ENT_CMDENUM_IDX + ' [4:nValue1] ENT_PARM1_IDX + ' [5:nValue2] ENT_PARM2_IDX + ' [6:nValue3] ENT_PARM3_IDX + ' [7:nValue4] ENT_PARM4_IDX + ' [etc] - up to 6 max values + + LONG [@currParsedCommand][ENT_RAWSTR_IDX] := @currSingleLineBffr ' record location of raw line + LONG [@currParsedCommand][ENT_VALUE_CT_IDX] := 0 ' preset not recognized + LONG [@currParsedCommand][ENT_CMDENUM_IDX] := 0 ' preset not recognized + debug("- cp: raw=[", zstr_(LONG [@currParsedCommand][ENT_RAWSTR_IDX]), "]") + ' + bytefill(@currCommandName, 0, RX_COMMAND_MAX_LEN+1) ' reset buffers + + pValues := @currParsedCommand + nStatus := OP_FAILURE ' preset to failure + + pSrchBffr := pRxLine + nLenRemaining := strsize(pRxLine) ' get length of input + nSepIdx := -1 ' separator not found + + ' locate our command string within rx buffer + pRemainingBffr, nSepIdx := locationNextNonWhiteValue(pSrchBffr, nLenRemaining) + + ' if we have non-empty line... + if (nLenRemaining > 0) + ' if we bounded the command name... + if (nSepIdx >= 0) + ' capture subsystem ID for this status + nCmdLen := (nSepIdx <= RX_COMMAND_MAX_LEN) ? nSepIdx : RX_COMMAND_MAX_LEN + bytemove(@currCommandName, pRemainingBffr, nCmdLen) + LONG [@currParsedCommand][ENT_CMDSTR_IDX] := @currCommandName ' store command name + LONG [@currParsedCommand][ENT_VALUE_CT_IDX] := 1 ' note that we have command name + debug("- cp: cmd=[", zstr_(LONG [@currParsedCommand][ENT_CMDSTR_IDX]), "]") + + ' point past command + pSrchBffr := pSrchBffr + nSepIdx + 1 ' +1 = point past sep + nLenRemaining := strsize(pSrchBffr) ' get length of input remainder + + nStatus := OP_SUCCESS + + ' if anything left, identify params + if (nLenRemaining > 0) + nParmIdx := ENT_PARM1_IDX ' first param is at [4] into structure + repeat + bytefill(@tmpValueStr, 0, RX_VALUE_MAX_LEN+1) ' reset work buffer + + pRemainingBffr, nSepIdx := locationNextNonWhiteValue(pSrchBffr, nLenRemaining) + + ' tmpValueStr byte 0[RX_VALUE_MAX_LEN+1] + ' save into terminated string + nCmdLen := (nSepIdx <= RX_VALUE_MAX_LEN) ? nSepIdx : RX_VALUE_MAX_LEN + if nCmdLen == 0 + quit + + bytemove(@tmpValueStr, pRemainingBffr, nCmdLen) + debug("- cp: valStr=[", zstr_(@tmpValueStr), "]") + ' convert string into value + if strHasLowCasePrefix(@tmpValueStr, @"true") + LONG [@currParsedCommand][nParmIdx] := TRUE + elseif strHasLowCasePrefix(@tmpValueStr, @"false") + LONG [@currParsedCommand][nParmIdx] := FALSE + else + LONG [@currParsedCommand][nParmIdx] := decimalForString(@tmpValueStr) + ' increment count of parms + debug("- cp: val=(", sdec_long_(LONG [@currParsedCommand][nParmIdx]), ")") + LONG [@currParsedCommand][ENT_VALUE_CT_IDX] += 1 + nParmIdx++ + pSrchBffr := pSrchBffr + nSepIdx + 1 ' +1 = point past sep + + if pRemainingBffr == 0 + quit + + 'dumpParsedCommand(@"cmdprse") + +PRI dumpParsedCommand(pMsg) | nIdx, nParams +' pValues -> [0:nbrValues(1-n)] (doesn't count "full string") ENT_VALUE_CT_IDX +' [1:pStr] -> "full string" ENT_RAWSTR_IDX +' [2:pCmd] -> "cmd" ENT_CMDSTR_IDX +' [3:eCmd] ENT_CMDENUM_IDX +' [4:nValue1] ENT_PARM1_IDX +' [5:nValue2] ENT_PARM2_IDX +' [6:nValue3] ENT_PARM3_IDX +' [7:nValue4] ENT_PARM4_IDX +' [etc] - up to 6 max values + debug("* Command: - ", zstr_(pMsg)) + debug(" -- raw=[", zstr_(LONG [@currParsedCommand][ENT_RAWSTR_IDX]), "]") + debug(" -- ct=(", udec_(LONG [@currParsedCommand][ENT_VALUE_CT_IDX]), ")") + debug(" -- cmd=[", zstr_(LONG [@currParsedCommand][ENT_CMDSTR_IDX]), "]") + debug(" -- eCmd=(", udec_(LONG [@currParsedCommand][ENT_CMDENUM_IDX]), ")") + nParams := LONG [@currParsedCommand][0] - 1 + if nParams > 0 + repeat nIdx from ENT_PARM1_IDX to ENT_PARM1_IDX + nParams - 1 + debug(" -- param#", udec_(nIdx), "=(", sdec_(LONG [@currParsedCommand][nIdx]), ")") + +PRI locationNextNonWhiteValue(pBffr, lenRemaining) : pStart, nSepIdx | pBffrStart, nLenRemaining, nIdx, nChr +' Return ptr and index of end char of next non-white value + nSepIdx := -1 ' separator not found + ' skip any starting white-space + pStart, nLenRemaining := skipWhiteSpace(pBffr, lenRemaining) + ' locate end of this non-white value + repeat nIdx from 0 to nLenRemaining + nChr := byte[pStart][nIdx] + ' if have space, tab or EOL... + if (nChr == $20 or nChr == $09 or nChr == $00) + nSepIdx := nIdx + quit + 'debug("- lnnwv: str=[", zstr_(pBffr), "] ", uhex_long(pBffr), udec_long(lenRemaining), " -> ", uhex_long(pStart), udec_long(nSepIdx)) + +PRI skipWhiteSpace(pBffr, lenRemaining) : pStart, nLenRemaining | nIdx, nChr +' Return ptr to first non-white char with adjusted len which is len of non-white to end of string + pStart := pBffr + nLenRemaining := lenRemaining + repeat nIdx from 0 to lenRemaining + nChr := byte[pBffr][nIdx] + ' if NOT have space, tab or EOL... + if (nChr <> $20 and nChr <> $09 and nChr <> $00) + pStart := @byte[pBffr][nIdx] + nLenRemaining := lenRemaining - nIdx + quit + 'debug("- sws: str=[", zstr_(pBffr), "] ", uhex_long(pBffr), udec_long(lenRemaining), " -> ", uhex_long(pStart), udec_long(nLenRemaining)) + +PRI strHasLowCasePrefix(pTargetStr, pPrefix) : bHasStatus | nIdx + bHasStatus := TRUE + repeat nIdx from 0 to strsize(pPrefix) - 1 + if (BYTE[pTargetStr][nIdx] | $20) <> BYTE[pPrefix][nIdx] + bHasStatus := False + quit ' outta here, we have our answer! + + +PRI strIgnoreCaseMatch(pLtStr, pRtStr) : bMatchStatus | nIdx + bMatchStatus := FALSE + 'debug("- sicm: lt(", udec_(strsize(pLtStr)), ")=[", zstr_(pLtStr), "], rt(", udec_(strsize(pRtStr)), ")=[", zstr_(pRtStr), "]") + if strsize(pLtStr) == strsize(pRtStr) + bMatchStatus := TRUE + repeat nIdx from 0 to strsize(pRtStr) - 1 + ' compare lowCase lt-byte to lowcase rt-byte + if (BYTE[pLtStr][nIdx] | $20) <> BYTE[pRtStr][nIdx] | $20 + bMatchStatus := FALSE + quit ' outta here, we have our answer! + 'debug(" -- ", sdec_(bMatchStatus)) + +PRI indexOfStr(pSrchStr, pRxBffr) : nIdx, idxAfter | nLenRemaining, srchLen, srchIdx, srcIdx, bSrchMatch + nIdx := -1 ' preset NOT found + nLenRemaining := strsize(pRxBffr) ' get length of input + srchLen := strsize(pSrchStr) + if (srchLen <= nLenRemaining) + repeat srcIdx from 0 to nLenRemaining - srchLen + bSrchMatch := True + repeat srchIdx from 0 to srchLen - 1 + if (byte[pSrchStr][srchIdx] <> byte[pRxBffr][srcIdx + srchIdx]) + bSrchMatch := False + quit ' abort srchStr compare + if bSrchMatch == True + nIdx := srcIdx + idxAfter := srcIdx + srchLen + quit ' abort rxBuffer compare + 'debug("idxOf: ", zstr(pSrchStr), ", ", zstr(pRxBffr), ", nIdx=", sdec_(nIdx)) + +CON { ----- TEST Support ----- } + +PRI prepStackForCheck() +' reset all failure indicators + + longfill(@taskStack, NOT_WRITTEN_MARK, STACK_SIZE_LONGS) + endStackMark := DO_NOT_WRITE_MARK + +PRI checkStack() + if endStackMark <> DO_NOT_WRITE_MARK + debug("^^^ STACK Overflow! Depth greater than ", udec(nStackDepth), " longs") + lockupForAnly() + +PRI reportStackUse() + nStkChkUsed := 0 + repeat nStkChkIdx from 0 to STACK_SIZE_LONGS - 1 + if LONG[@taskStack][nStkChkIdx] <> NOT_WRITTEN_MARK + nStkChkUsed++ + else + quit ' done we have our count + debug("^^^ STACK used ", udec(nStkChkUsed), " of ", udec(nStackDepth)) + +PRI testReport(pTestId, bPassFail) | pResult + pResult := (bPassFail == TST_PASS) ? @rsltPass : @rsltFail + debug("+++ ---------") + debug("+++ TEST [", zstr_(pTestId), "] - ", zstr_(pResult)) + reportStackUse() + checkStack() + 'dumpStack() + + if bPassFail == TST_FAIL + lockupForAnly() + +PRI dumpStack() + dbgMemDump(@taskStack, (STACK_SIZE_LONGS + 1) * 4, string("TASK Stack")) + +DAT { test message things } + +testMsgCt LONG 0 + +testMsgStr BYTE "Test " +testMsgNbr BYTE "000x" +testMsgEOL BYTE $0a, 0 + +rsltPass BYTE "pass",0 +rsltFail BYTE "FAIL",0 + +nStkChkUsed LONG 0 +nStkChkIdx LONG 0 + +PRI genTestMsg() : pMsg | nDigit, nRemainder +' return pointer to a simple message + pMsg := @testMsgStr + nRemainder := testMsgCt++ + if nRemainder > 999 + nDigit := nRemainder / 1000 + nRemainder -= nDigit * 1000 + BYTE[@testMsgNbr][0] := nDigit + $30 + if nRemainder > 99 + nDigit := nRemainder / 100 + nRemainder -= nDigit * 100 + BYTE[@testMsgNbr][1] := nDigit + $30 + if nRemainder > 9 + nDigit := nRemainder / 10 + nRemainder -= nDigit * 10 + BYTE[@testMsgNbr][2] := nDigit + $30 + BYTE[@testMsgNbr][3] := nRemainder + $30 + +PRI showTestState(ePrior, eNew) | pFrom, pTo +' display task state + pFrom := nameForState(ePrior) + pTo := nameForState(eNew) + debug("TEST state [", zstr_(pFrom), "] -> [", zstr_(pTo), "]") + +PRI nameForState(eNew) : pStr +' return string for state value + if eNew == CT_IDLE + pStr := @ctStrIdle + elseif eNew == CT_STOP + pStr := @ctStrStop + elseif eNew == CT_PLACE_STR + pStr := @ctStrPlcStr + elseif eNew == CT_PLACE_N_MSGS + pStr := @ctStrPlcNMsgs + else + pStr := @ctStrUnknown + +PRI dumpStrQ(pMsg) +' dump our full string-que so we can visually inspect + dbgMemDump(@nQStrHeadIdx, (4 + RX_STR_Q_MAX_LONGS) * 4, pMsg) + +PRI dbgMemDump(pBytes, lenBytes, pMessage) | rowCount, rowLen, pCurrByte, lastRowByteCount, bytesSoFar +' Dump rows of hex values with address preceeding + + if pMessage + debug("** ", zstr_(pMessage), ":") + + rowCount := lenBytes / 16 + lastRowByteCount := lenBytes - (rowCount * 16) + pCurrByte := pBytes + bytesSoFar := 0 + + ' emit full lines + if rowCount > 0 + repeat rowCount + dbgMemDumpRow(pCurrByte, 16) + pCurrByte += 16 + bytesSoFar += 16 + + if bytesSoFar < lenBytes + ' emit last line + dbgMemDumpRow(pCurrByte, lastRowByteCount) + +PRI dbgMemDumpRow(pBytes, lenBytes) '| rowCount, rowLen, pCurrByte, bytIndex +' emit address followed by bytes + debug(" ", uhex_long_(pBytes), ": ", uhex_byte_array_(pBytes, lenBytes)) + + +CON { license } +{{ + + ------------------------------------------------------------------------------------------------- + MIT License + + Copyright (c) 2022 Iron Sheep Productions, LLC + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + ================================================================================================= +}} diff --git a/isp_serial.spin2 b/isp_serial.spin2 new file mode 100644 index 0000000..ac5a6c7 --- /dev/null +++ b/isp_serial.spin2 @@ -0,0 +1,628 @@ +'' ================================================================================================= +'' +'' File....... isp_serial.spin2 +'' Purpose.... True mode, unbuffererd serial coms using smart pins +'' -- same features as original jm_serial.spin2 by Jon McPhalen with minor enhancements +'' Authors.... Eric Smith, Chip Gracey, and Jon McPhalen +'' -- formatted strings inspired by clib.spin for P1 (author unknown) +'' -- see below for terms of use +'' E-mail..... stephen@ironsheep.biz +'' Started.... Feb 2022 +'' Updated.... 4 Feb 2022 +'' +'' ================================================================================================= + +'' stop bit support added by Stephen M Moraco 13 Jan 2022 + +CON { fixed io pins } + + RX1 = 63 + TX1 = 62 + + +CON { smart pin modes } + + TX_MODE = %0000_0000_000_0000000000000_01_11110_0 + RX_MODE = %0000_0000_000_0000000000000_00_11111_0 + +CON { serial configuration } + + DATA_BITS = 8 + STOP_BITS = 2 + + ' (following ignored for now) + #0, PARITY_NONE, PARITY_EVEN, PARITY_ODD + + ' tx pin pull-up constants + #0, PU_NONE, PU_1K5, PU_3K3, PU_15K ' pull-up options + + +CON { pst formatting } + + HOME = 1 + CRSR_XY = 2 + CRSR_LF = 3 + CRSR_RT = 4 + CRSR_UP = 5 + CRSR_DN = 6 + BELL = 7 + BKSP = 8 + TAB = 9 + LF = 10 + CLR_EOL = 11 + CLR_DN = 12 + CR = 13 + CRSR_X = 14 + CRSR_Y = 15 + CLS = 16 + + +OBJ + + nstr : "jm_nstr" + +VAR + + long rxp + long txp + long baud + + long byteMask + long byteExtraBits + + byte pbuf[80] ' padded strings + + +PUB null() + + '' This is not a top level object + + +PUB start(baudrate) + +'' Start simple serial coms on default pins at baudrate + + startx(RX1, TX1, baudrate, P_HIGH_15K) + + +PUB startx(rxpin, txpin, baudrate, txPullup) | bitperiod, bitmode, databits + +'' Start simple serial coms on rxpin and txpin at baudrate + + longmove(@rxp, @rxpin, 3) ' also save baudrate incase driver is asked + bitperiod := clkfreq / baudrate + + case txPullup + PU_NONE : txPullup := P_HIGH_FLOAT ' use external pull-up + PU_1K5 : txPullup := P_HIGH_1K5 ' 1.5k + PU_3K3 : txPullup := P_HIGH_1MA ' acts like ~3.3k + other : txPullup := P_HIGH_15K ' 15K + + databits := DATA_BITS + if (STOP_BITS == 2) + databits += 1 + + bitmode := 7 + (bitperiod << 16) + + ' setup constants for tx() to use + byteMask := $FF + if (DATA_BITS == 7) + byteMask := $7f + + byteExtraBits := 0 + if (STOP_BITS == 2) + byteExtraBits |= $100 + + pinf(txp) + wrpin(txp, TX_MODE | txPullup) + wxpin(txp, bitmode) + pinl(txp) + + pinf(rxp) + wrpin(rxp, RX_MODE) + wxpin(rxp, bitmode) + pinl(rxp) + +PUB getBaudrate() : baudrate +'' Return the configured baud rate + baudrate := baud '' was saved by startx() + +PUB rxflush() + +'' Clear serial input + + repeat + while (rxcheck() >= 0) + + +PUB rxcheck() : rxbyte | check + +'' Check for serial input +'' -- returns -1 if nothing available + + rxbyte := -1 + check := pinr(rxp) + if (check) + rxbyte := rdpin(rxp) >> 24 + rxbyte &= byteMask ' handle 7bit data (e.g., 7N1 vs. 8N1) + + +PUB rxtime(ms) : b | mstix, t + +'' Wait ms milliseconds for a byte to be received +'' -- returns -1 if no byte received, $00..$FF if byte + + mstix := clkfreq / 1000 + + t := getct() + repeat + until ((b := rxcheck()) >= 0) or (((getct() - t) / mstix) > ms) + + +PUB rx() : rxbyte + +'' Wait for serial input +'' -- blocks! + + repeat + rxbyte := rxcheck() + until (rxbyte >= 0) + + +PUB tx(b) | byt, mask + +'' Emit byte + + b &= byteMask + b |= byteExtraBits + + wypin(txp, b) + txflush() + + +PUB txn(b, n) + +'' Emit byte n times + + repeat n + tx(b) + + +PUB txflush() | check + +'' Wait until last byte has finished + + repeat + check := pinr(txp) + while (check == 0) + + +PUB str(p_str) + +'' Emit z-string at p_str + + repeat (strsize(p_str)) + tx(byte[p_str++]) + + +PUB substr(p_str, len) | b + +'' Emit len characters of string at p_str +'' -- aborts if end of string detected + + repeat len + b := byte[p_str++] + if (b > 0) + tx(b) + else + quit + + +PUB padstr(p_str, width, pad) + +'' Emit p_str as padded field of width characters +'' -- pad is character to use to fill out field +'' -- positive width causes right alignment +'' -- negative width causes left alignment + + str(nstr.padstr(p_str, width, pad)) + + +CON { formatted strings } + +{{ + Escape sequences + + \\ backslash char + \% percent char + \r carriage return + \n new line (vertical tab) + \t tab (horizontal) + \q double quote + \nnn arbitrary ASCII value (nnn is decimal) + + Formatted arguments + + %w.pf print argument as decimal width decimal point + %[w[.p]]d print argument as decimal + %[w[.p]]x print argument as hex + %[w[.p]]o print argument as octal + %[w[.p]]q print argument as quarternary + %[w[.p]]b print argument as binary + %[w]s print argument as string + %[w]c print argument as character ( + + -- w is field width + * positive n causes right alignment in field + * negative n causes left align ment in field + -- %ns aligns s in field (may truncate) + -- %nc prints n copies of c + -- p is precision characters + * number of characters to use, aligned in field + -- for %n.pf, p is number of digits after dpoint + +}} + + +PUB fstr0(p_str) + +'' Emit string with formatting characters. + + format(p_str, 0) + + +PUB fstr1(p_str, arg1) + +'' Emit string with formatting characters and one argument. + + format(p_str, @arg1) + + +PUB fstr2(p_str, arg1, arg2) + +'' Emit string with formatting characters and two arguments. + + format(p_str, @arg1) + + +PUB fstr3(p_str, arg1, arg2, arg3) + +'' Emit string with formatting characters and three arguments. + + format(p_str, @arg1) + + +PUB fstr4(p_str, arg1, arg2, arg3, arg4) + +'' Emit string with formatting characters and four arguments. + + format(p_str, @arg1) + + +PUB fstr5(p_str, arg1, arg2, arg3, arg4, arg5) + +'' Emit string with formatting characters and five arguments. + + format(p_str, @arg1) + + +PUB fstr6(p_str, arg1, arg2, arg3, arg4, arg5, arg6) + +'' Emit string with formatting characters and six arguments. + + format(p_str, @arg1) + +PUB fstr7(p_str, arg1, arg2, arg3, arg4, arg5, arg6, arg7) + +'' Emit string with formatting characters and seven arguments. + + format(p_str, @arg1) + + +PUB format(p_str, p_args) | idx, c, asc, field, digits + +'' Emit formatted string with escape sequences and embedded values +'' -- p_str is a pointer to the format control string +'' -- p_args is pointer to array of longs that hold field values +'' * field values can be numbers, characters, or pointers to strings + + idx := 0 ' value index + + repeat + c := byte[p_str++] + if (c == 0) + return + + elseif (c == "\") + c := byte[p_str++] + if (c == "\") + tx("\") + elseif (c == "%") + tx("%") + elseif (c == "r") + tx(CR) + elseif (c == "n") + tx(LF) + elseif (c == "t") + tx(TAB) + elseif (c == "q") + tx(34) + elseif ((c >= "0") and (c <= "9")) + --p_str + p_str, asc, _ := get_nargs(p_str) + if ((asc >= 0) and (asc <= 255)) + tx(asc) + + elseif (c == "%") + p_str, field, digits := get_nargs(p_str) + c := byte[p_str++] + if (c == "f") + str(nstr.fmt_number(long[p_args][idx++], 99, digits, field, " ")) + elseif (c == "d") + str(nstr.fmt_number(long[p_args][idx++], 10, digits, field, " ")) + elseif (c == "x") + str(nstr.fmt_number(long[p_args][idx++], 16, digits, field, " ")) + elseif (c == "o") + str(nstr.fmt_number(long[p_args][idx++], 08, digits, field, " ")) + elseif (c == "q") + str(nstr.fmt_number(long[p_args][idx++], 04, digits, field, " ")) + elseif (c == "b") + str(nstr.fmt_number(long[p_args][idx++], 02, digits, field, " ")) + elseif (c == "s") + str(nstr.padstr(long[p_args][idx++], field, " ")) + elseif (c == "c") + txn(long[p_args][idx++], abs field) + + else + tx(c) + + +PRI get_nargs(p_str) : p_str1, val1, val2 | c, sign + +'' Parse one or two numbers from string in n, -n, n.n, or -n.n format +'' -- dpoint separates values +'' -- only first # may be negative +'' -- returns pointer to 1st char after value(s) + + c := byte[p_str] ' check for negative on first value + if (c == "-") + sign := -1 + ++p_str + else + sign := 0 + + repeat ' get first value + c := byte[p_str++] + if ((c >= "0") and (c <= "9")) + val1 := (val1 * 10) + (c - "0") + else + if (sign) + val1 := -val1 + quit + + if (c == ".") ' if dpoint + repeat ' get second value + c := byte[p_str++] + if ((c >= "0") and (c <= "9")) + val2 := (val2 * 10) + (c - "0") + else + quit + + p_str1 := p_str - 1 ' back up to non-digit + + +PUB fmt_number(value, base, digits, width, pad) + +'' Emit value converted to number in padded field +'' -- value is converted using base as radix +'' * 99 for decimal with digits after decimal point +'' -- digits is max number of digits to use +'' -- width is width of final field (max) +'' -- pad is character that fills out field + + str(nstr.fmt_number(value, base, digits, width, pad)) + + +PUB dec(value) + +'' Emit value as decimal + +' str(nstr.dec(value, 0)) + str(nstr.itoa(value, 10, 0)) + + +PUB fwdec(value, digits) + +'' Emit value as decimal using fixed # of digits +'' -- may add leading zeros + +' str(nstr.dec(value, digits)) + str(nstr.itoa(value, 10, digits)) + + +PUB jdec(value, digits, width, pad) + +'' Emit value as decimal using fixed # of digits +'' -- aligned in padded field (negative width to left-align) +'' -- digits is max number of digits to use +'' -- width is width of final field (max) +'' -- pad is character that fills out field + + str(nstr.fmt_number(value, 10, digits, width, pad)) + + +PUB dpdec(value, dp) + +'' Emit value as decimal with decimal point +'' -- dp is number of digits after decimal point + + str(nstr.dpdec(value, dp)) + + + +PUB jdpdec(value, dp, width, pad) + +'' Emit value as decimal with decimal point +'' -- aligned in padded field (negative width to left-align) +'' -- dp is number of digits after decimal point +'' -- width is width of final field (max) +'' -- pad is character that fills out field + + str(nstr.fmt_number(value, 99, dp, width, pad)) + + +PUB hex(value) + +'' Emit value as hexadecimal + + str(nstr.itoa(value, 16, 0)) + + +PUB fwhex(value, digits) + +'' Emit value as hexadecimal using fixed # of digits + + str(nstr.itoa(value, 16, digits)) + + +PUB jhex(value, digits, width, pad) + +'' Emit value as quarternary using fixed # of digits +'' -- aligned inside field +'' -- pad fills out field + + str(nstr.fmt_number(value, 16, digits, width, pad)) + + +PUB oct(value) + +'' Emit value as octal + + str(nstr.itoa(value, 8, 0)) + + +PUB fwoct(value, digits) + +'' Emit value as octal using fixed # of digits + + str(nstr.itoa(value, 8, digits)) + + +PUB joct(value, digits, width, pad) + +'' Emit value as octal using fixed # of digits +'' -- aligned inside field +'' -- pad fills out field + + str(nstr.fmt_number(value, 8, digits, width, pad)) + + +PUB qrt(value) + +'' Emit value as quarternary + + str(nstr.itoa(value, 4, 0)) + + +PUB fwqrt(value, digits) + +'' Emit value as quarternary using fixed # of digits + + str(nstr.itoa(value, 4, digits)) + + +PUB jqrt(value, digits, width, pad) + +'' Emit value as quarternary using fixed # of digits +'' -- aligned inside field +'' -- pad fills out field + + str(nstr.fmt_number(value, 4, digits, width, pad)) + + +PUB bin(value) + +'' Emit value as binary + + str(nstr.itoa(value, 2, 0)) + + +PUB fwbin(value, digits) + +'' Emit value as binary using fixed # of digits + + str(nstr.itoa(value, 2, digits)) + + +PUB jbin(value, digits, width, pad) + +'' Emit value as binary using fixed # of digits +'' -- aligned inside field +'' -- pad fills out field + + str(nstr.fmt_number(value, 2, digits, width, pad)) + +PUB memDump(pBytes, lenBytes, pMessage) | rowCount, rowLen, pCurrByte, lastRowByteCount, bytesSoFar + +'' Dump rows of hex values with address preceeding + + if pMessage + fstr1(string("** %s:\r\n"), pMessage) + + rowCount := lenBytes / 16 + lastRowByteCount := lenBytes - (rowCount * 16) + pCurrByte := pBytes + bytesSoFar := 0 + + ' emit full lines + if rowCount > 0 + repeat rowCount + memDumpRow(pCurrByte, 16) + pCurrByte += 16 + bytesSoFar += 16 + + if bytesSoFar < lenBytes + ' emit last line + memDumpRow(pCurrByte, lastRowByteCount) + +pri memDumpRow(pBytes, lenBytes) | rowCount, rowLen, pCurrByte, bytIndex + ' emit address + fstr1(string(" 0x%.8x: "), pBytes) + ' emit 8 bytes + ' emit gap + ' emit 8 bytes + repeat bytIndex from 0 to lenBytes - 1 + fstr1(string("0x%.2x "), BYTE [pBytes][bytIndex]) + if bytIndex == 7 + fstr0(string(" ")) + fstr0(string("\r\n")) + + + +CON { license } +{{ + + ------------------------------------------------------------------------------------------------- + MIT License + + Copyright (c) 2022 Iron Sheep Productions, LLC + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + ================================================================================================= +}} diff --git a/isp_serial_singleton.spin2 b/isp_serial_singleton.spin2 index 6143181..66e475c 100644 --- a/isp_serial_singleton.spin2 +++ b/isp_serial_singleton.spin2 @@ -22,7 +22,7 @@ } -con { fixed io pins } +CON { fixed io pins } RX1 = 63 { I } ' programming / debug TX1 = 62 { O } @@ -33,7 +33,23 @@ con { fixed io pins } SF_SDI = 58 { I } -con { pst formatting } +CON { smart pin modes } + + TX_MODE = %0000_0000_000_0000000000000_01_11110_0 + RX_MODE = %0000_0000_000_0000000000000_00_11111_0 + +CON { serial configuration } + + DATA_BITS = 8 + STOP_BITS = 2 + + ' (following ignored for now) + #0, PARITY_NONE, PARITY_EVEN, PARITY_ODD + + ' tx pin pull-up constants + #0, PU_NONE, PU_1K5, PU_3K3, PU_15K ' pull-up options + +CON { pst formatting } HOME = 1 CRSR_XY = 2 @@ -53,66 +69,84 @@ con { pst formatting } CLS = 16 -obj +OBJ nstr : "jm_nstr" ' number-to-string -dat +DAT -rxp long 0 -txp long 0 +rxp long 0 +txp long 0 +baud long 0 -pbuf byte 0[80] ' padded strings +byteMask long 0 +byteExtraBits long 0 -kLock byte 0 +pbuf byte 0[80] ' padded strings -pub null() +kLock byte 0 - '' This is not a top level object +PUB null() + '' This is not a top level object -pub start(baudrate) +PUB start(baudrate) '' Start simple serial coms on default pins at baudrate - startx(RX1, TX1, baudrate) ' use programming port + startx(RX1, TX1, baudrate, P_HIGH_15K) ' use programming port -pub startx(rxpin, txpin, baud) | bitmode - +PUB startx(rxpin, txpin, baudrate, txPullup) | bitperiod, bitmode, databits '' Start simple serial coms on rxpin and txpin at baud - 'longmove(@rxp, @rxpin, 2) ' save pins - rxp := rxpin - txp := txpin + longmove(@rxp, @rxpin, 3) ' also save baudrate incase driver is asked + bitperiod := clkfreq / baudrate + + case txPullup + PU_NONE : txPullup := P_HIGH_FLOAT ' use external pull-up + PU_1K5 : txPullup := P_HIGH_1K5 ' 1.5k + PU_3K3 : txPullup := P_HIGH_1MA ' acts like ~3.3k + other : txPullup := P_HIGH_15K ' 15K + + databits := DATA_BITS + if (STOP_BITS == 2) + databits += 1 + + bitmode := 7 + (bitperiod << 16) + ' setup constants for tx() to use + byteMask := $FF + if (DATA_BITS == 7) + byteMask := $7f - bitmode := muldiv64(clkfreq, $1_0000, baud) & $FFFFFC00 ' set bit timing - bitmode |= 7 ' set bits (8) + byteExtraBits := 0 + if (STOP_BITS == 2) + byteExtraBits |= $100 - org - fltl rxpin ' configure rx smart pin - wrpin ##P_ASYNC_RX, rxpin - wxpin bitmode, rxpin - drvl rxpin - fltl txpin ' configure tx smart pin - wrpin ##(P_ASYNC_TX | P_OE), txpin - wxpin bitmode, txpin - drvl txpin - end + pinf(txp) + wrpin(txp, TX_MODE | txPullup) + wxpin(txp, bitmode) + pinl(txp) + pinf(rxp) + wrpin(rxp, RX_MODE) + wxpin(rxp, bitmode) + pinl(rxp) -pub rxflush() +PUB getBaudrate() : baudrate +'' Return the configured baud rate + baudrate := baud '' was saved by startx() +PUB rxflush() '' Clear serial input repeat while (rxcheck() >= 0) -pub rxcheck() : rxbyte | check - +PUB rxcheck() : rxbyte | check '' Check for serial input '' -- returns -1 if nothing available @@ -120,10 +154,9 @@ pub rxcheck() : rxbyte | check check := pinr(rxp) if (check) rxbyte := rdpin(rxp) >> 24 + rxbyte &= byteMask ' handle 7bit data (e.g., 7N1 vs. 8N1) - -pub rxtime(ms) : b | mstix, t - +PUB rxtime(ms) : b | mstix, t '' Wait ms milliseconds for a byte to be received '' -- returns -1 if no byte received, $00..$FF if byte @@ -134,7 +167,7 @@ pub rxtime(ms) : b | mstix, t until ((b := rxcheck()) >= 0) or (((getct() - t) / mstix) > ms) -pub rx() : rxbyte +PUB rx() : rxbyte '' Wait for serial input '' -- blocks! @@ -144,15 +177,18 @@ pub rx() : rxbyte until (rxbyte >= 0) -pub tx(b) +PUB tx(b) '' Emit byte + b &= byteMask + b |= byteExtraBits + wypin(txp, b) txflush() -pub txn(b, n) +PUB txn(b, n) '' Emit byte n times @@ -160,7 +196,7 @@ pub txn(b, n) tx(b) -pub txflush() | check +PUB txflush() | check '' Wait until last byte has finished @@ -169,7 +205,7 @@ pub txflush() | check while (check == 0) -pub str(p_str) +PUB str(p_str) '' Emit z-string at p_str @@ -177,7 +213,7 @@ pub str(p_str) tx(byte[p_str++]) -pub substr(p_str, len) | b +PUB substr(p_str, len) | b '' Emit len characters of string at p_str '' -- aborts if end of string detected @@ -190,7 +226,7 @@ pub substr(p_str, len) | b quit -pub padstr(p_str, width, pad) +PUB padstr(p_str, width, pad) '' Emit p_str as padded field of width characters '' -- pad is character to use to fill out field @@ -200,7 +236,7 @@ pub padstr(p_str, width, pad) str(nstr.padstr(p_str, width, pad)) -con { formatted strings } +CON { formatted strings } {{ Escaped characters @@ -237,62 +273,62 @@ con { formatted strings } }} -pub fstr0(p_str) +PUB fstr0(p_str) '' Emit string with formatting characters. format(p_str, 0) -pub fstr1(p_str, arg1) +PUB fstr1(p_str, arg1) '' Emit string with formatting characters and one argument. format(p_str, @arg1) -pub fstr2(p_str, arg1, arg2) +PUB fstr2(p_str, arg1, arg2) '' Emit string with formatting characters and two arguments. format(p_str, @arg1) -pub fstr3(p_str, arg1, arg2, arg3) +PUB fstr3(p_str, arg1, arg2, arg3) '' Emit string with formatting characters and three arguments. format(p_str, @arg1) -pub fstr4(p_str, arg1, arg2, arg3, arg4) +PUB fstr4(p_str, arg1, arg2, arg3, arg4) '' Emit string with formatting characters and four arguments. format(p_str, @arg1) -pub fstr5(p_str, arg1, arg2, arg3, arg4, arg5) +PUB fstr5(p_str, arg1, arg2, arg3, arg4, arg5) '' Emit string with formatting characters and five arguments. format(p_str, @arg1) -pub fstr6(p_str, arg1, arg2, arg3, arg4, arg5, arg6) +PUB fstr6(p_str, arg1, arg2, arg3, arg4, arg5, arg6) '' Emit string with formatting characters and six arguments. format(p_str, @arg1) -pub fstr7(p_str, arg1, arg2, arg3, arg4, arg5, arg6, arg7) +PUB fstr7(p_str, arg1, arg2, arg3, arg4, arg5, arg6, arg7) '' Emit string with formatting characters and seven arguments. format(p_str, @arg1) -pub format(p_str, p_args) | idx, c, asc, field, digits, kindaLock +PUB format(p_str, p_args) | idx, c, asc, field, digits, kindaLock '' Emit formatted string with escape sequences and embedded values '' -- p_str is a pointer to the format control string @@ -397,7 +433,7 @@ pri get_nargs(p_str) : p_str1, val1, val2 | c, sign p_str1 := p_str - 1 ' back up to non-digit -pub fmt_number(value, base, digits, width, pad) +PUB fmt_number(value, base, digits, width, pad) '' Emit value converted to number in padded field '' -- value is converted using base as radix @@ -409,14 +445,14 @@ pub fmt_number(value, base, digits, width, pad) str(nstr.fmt_number(value, base, digits, width, pad)) -pub dec(value) +PUB dec(value) '' Emit value as decimal str(nstr.itoa(value, 10, 0)) -pub fwdec(value, digits) +PUB fwdec(value, digits) '' Emit value as decimal using fixed # of digits '' -- may add leading zeros @@ -424,7 +460,7 @@ pub fwdec(value, digits) str(nstr.itoa(value, 10, digits)) -pub jdec(value, digits, width, pad) +PUB jdec(value, digits, width, pad) '' Emit value as decimal using fixed # of digits '' -- aligned in padded field (negative width to left-align) @@ -435,7 +471,7 @@ pub jdec(value, digits, width, pad) str(nstr.fmt_number(value, 10, digits, width, pad)) -pub dpdec(value, dp) +PUB dpdec(value, dp) '' Emit value as decimal with decimal point '' -- dp is number of digits after decimal point @@ -443,7 +479,7 @@ pub dpdec(value, dp) str(nstr.dpdec(value, dp)) -pub jdpdec(value, dp, width, pad) +PUB jdpdec(value, dp, width, pad) '' Emit value as decimal with decimal point '' -- aligned in padded field (negative width to left-align) @@ -454,21 +490,21 @@ pub jdpdec(value, dp, width, pad) str(nstr.fmt_number(value, 99, dp, width, pad)) -pub hex(value) +PUB hex(value) '' Emit value as hexadecimal str(nstr.itoa(value, 16, 0)) -pub fwhex(value, digits) +PUB fwhex(value, digits) '' Emit value as hexadecimal using fixed # of digits str(nstr.itoa(value, 16, digits)) -pub jhex(value, digits, width, pad) +PUB jhex(value, digits, width, pad) '' Emit value as quarternary using fixed # of digits '' -- aligned inside field @@ -477,21 +513,21 @@ pub jhex(value, digits, width, pad) str(nstr.fmt_number(value, 16, digits, width, pad)) -pub oct(value) +PUB oct(value) '' Emit value as octal str(nstr.itoa(value, 8, 0)) -pub foct(value, digits) +PUB foct(value, digits) '' Emit value as octal using fixed # of digits str(nstr.itoa(value, 8, digits)) -pub joct(value, digits, width, pad) +PUB joct(value, digits, width, pad) '' Emit value as octal using fixed # of digits '' -- aligned inside field @@ -500,21 +536,21 @@ pub joct(value, digits, width, pad) str(nstr.fmt_number(value, 8, digits, width, pad)) -pub qrt(value) +PUB qrt(value) '' Emit value as quarternary str(nstr.itoa(value, 4, 0)) -pub fqrt(value, digits) +PUB fqrt(value, digits) '' Emit value as quarternary using fixed # of digits str(nstr.itoa(value, 4, digits)) -pub jqrt(value, digits, width, pad) +PUB jqrt(value, digits, width, pad) '' Emit value as quarternary using fixed # of digits '' -- aligned inside field @@ -523,21 +559,21 @@ pub jqrt(value, digits, width, pad) str(nstr.fmt_number(value, 4, digits, width, pad)) -pub bin(value) +PUB bin(value) '' Emit value as binary str(nstr.itoa(value, 2, 0)) -pub fwbin(value, digits) +PUB fwbin(value, digits) '' Emit value as binary using fixed # of digits str(nstr.itoa(value, 2, digits)) -pub jbin(value, digits, width, pad) +PUB jbin(value, digits, width, pad) '' Emit value as binary using fixed # of digits '' -- aligned inside field @@ -545,7 +581,7 @@ pub jbin(value, digits, width, pad) str(nstr.fmt_number(value, 2, digits, width, pad)) -pub memDump(pBytes, lenBytes, pMessage) | rowCount, rowLen, pCurrByte, lastRowByteCount, bytesSoFar +PUB memDump(pBytes, lenBytes, pMessage) | rowCount, rowLen, pCurrByte, lastRowByteCount, bytesSoFar '' Dump rows of hex values with address preceeding @@ -581,7 +617,7 @@ pri memDumpRow(pBytes, lenBytes) | rowCount, rowLen, pCurrByte, bytIndex fstr0(string("\r\n")) -con { license } +CON { license } {{ diff --git a/isp_steering_2wheel.spin2 b/isp_steering_2wheel.spin2 index 8756497..a21f057 100644 --- a/isp_steering_2wheel.spin2 +++ b/isp_steering_2wheel.spin2 @@ -1,13 +1,13 @@ '' ================================================================================================= '' '' File....... isp_steering_2wheel.spin2 -'' Purpose.... Object providing control interface for stearing a twin-bldc-motor platform +'' Purpose.... Object providing control interface for steering a twin-bldc-motor platform '' Authors.... Stephen M Moraco '' -- Copyright (c) 2022 Iron Sheep Productions, LLC '' -- see below for terms of use '' E-mail..... stephen@ironsheep.biz '' Started.... Feb 2022 -'' Updated.... 9 Feb 2022 +'' Updated.... 4 May 2022 '' '' ================================================================================================= CON { forward our interface constants } @@ -35,6 +35,8 @@ CON { forward our interface constants } DDU_CM = ltWheel.DDU_CM DDU_FT = ltWheel.DDU_FT DDU_M = ltWheel.DDU_M + DDU_KM = ltWheel.DDU_KM + DDU_MI = ltWheel.DDU_MI ' Driver Rotation-Units Enum DRU_Unknown = ltWheel.DRU_Unknown @@ -53,14 +55,27 @@ CON { forward our interface constants } DS_HOLDING = ltWheel.DS_HOLDING DS_OFF = ltWheel.DS_OFF + ' Driver State Enum + DCS_Unknown = ltWheel.DCS_Unknown + DCS_STOPPED = ltWheel.DCS_STOPPED + DCS_SPIN_UP = ltWheel.DCS_SPIN_UP + DCS_AT_SPEED = ltWheel.DCS_AT_SPEED + DCS_SPIN_DN = ltWheel.DCS_SPIN_DN + DCS_SLOWING_DN = ltWheel.DCS_SLOWING_DN + DCS_SLOW_TO_CHG = ltWheel.DCS_SLOW_TO_CHG + DCS_FAULTED = ltWheel.DCS_FAULTED + DCS_ESTOP = ltWheel.DCS_ESTOP + + CON { test pins for LA measurement } +{ ' PIN_56 - PIN_63 TEST_BASE_PIN = 56 - TEST_ALL_PINS = TEST_BASE_PIN addpins 7 + TEST_PINS_ALL = TEST_BASE_PIN addpins 7 TEST_PIN_SNS_LOOP = TEST_BASE_PIN + 0 ' LA grey TEST_PIN_SNS_LP_ACTV = TEST_BASE_PIN + 1 ' LA red - +'} OBJ { our Motors } @@ -69,7 +84,6 @@ OBJ { our Motors } user : "isp_bldc_motor_userconfig" ' driver configuration distConv : "isp_dist_utils" ' distance conversion utils - PUB null() '' This is not a top-level object @@ -83,10 +97,10 @@ PUB start(leftBasePin, rightBasePin, driveVoltage) : ok circInMM_x10 := distConv.circInMMforDiaInInchFloat(user.MOTOR_DIA_IN_INCH) tickInMM_x10 := circInMM_x10 / 90 - - if clkfreq <> 270_000_000 - debug("!! ERROR bad CLOCK value") - repeat ' halt here + ' iff HDMI is needed, then we need this! + 'if clkfreq <> 270_000_000 + ' debug("!! ERROR bad CLOCK value") + ' repeat ' halt here ltWheel.start(leftBasePin, driveVoltage) rtWheel.start(rightBasePin, driveVoltage) @@ -106,12 +120,12 @@ PUB stop() rtWheel.stop() PUB setAcceleration(rate) -'' Limit Acceleration to {rate} +'' NOT WORKING: Limit Acceleration to {rate} where {rate} is [??? - ???] mm/s squared (default is ??? mm/s squared) ltWheel.setAcceleration(rate) rtWheel.setAcceleration(rate) PUB setMaxSpeed(speed) -'' Limit top-speed to {speed} +'' Limit top-speed to {speed} ltWheel.setMaxSpeed(speed) rtWheel.setMaxSpeed(speed) @@ -121,20 +135,21 @@ PUB setMaxSpeedForDistance(speed) rtWheel.setMaxSpeedForDistance(speed) PUB calibrate() +'' NOT WORKING: (we may need this?) '' have motor drivers determine fixed-offset constants ltWheel.calibrate() rtWheel.calibrate() PUB holdAtStop(bEnable) -'' Informs the motor drivers to actively hold postiion (bEnable=true) or coast (bEnable=false) at end of motion +'' Informs the motor drivers to actively hold position (bEnable=true) or coast (bEnable=false) at end of motion ltWheel.holdAtStop(bEnable) rtWheel.holdAtStop(bEnable) PUB resetTracking() '' Resets the position tracking values returned by getDistance() and getRotations() -'' Effectively: use current postion as home from now on - ltWheel.resetTracking() - rtWheel.resetTracking() +'' Effectively: use current position as home from now on + resetAccumLeft() + resetAccumRight() CON { --- Subsystem Control --- } @@ -227,7 +242,7 @@ PUB stopAfterDistance(nDistance, eDistanceUnits) | fValue abort '' Stops both motors, after either of the motors reaches {distance} specified in {distanceUnits} [DDU_IN or DDU_MM]. -''USE WITH: driveDirection(), drive() +'' USE WITH: driveDirection(), drive() PUB stopAfterTime(nTime, eTimeUnits) | timeNow '' Stops the motors after {time} specified in {timeUnits} [DTU_MILLISEC or DTU_SEC] has elapsed. '' USE WITH: driveAtPower() @@ -278,14 +293,53 @@ PUB clearEmergency() CON { --- Subsystem Status --- } PUB getDistance(eDistanceUnits) : leftDistanceInUnits, rightDistanceInUnits -'' Returns the distance in {distanceUnits} [DDU_IN or DDU_MM] travelled by each motor since last reset - leftDistanceInUnits := ltWheel.getDistance(eDistanceUnits) - rightDistanceInUnits := rtWheel.getDistance(eDistanceUnits) +'' Returns the distance in {distanceUnits} [DDU_MM, DDU_CM, DDU_IN, DDU_FT, DDU_M, DDU_KM, DDU_MI] travelled by each motor since last reset + debug("- gdi ", sdec_long(ltPosTrkHallTicks), sdec_long(rtPosTrkHallTicks)) + leftDistanceInUnits := convertDistance(ltPosTrkHallTicks, eDistanceUnits) + rightDistanceInUnits := convertDistance(rtPosTrkHallTicks, eDistanceUnits) + +PRI convertDistance(nValue, eDistanceUnits) : nDistanceInUnits | fMMpTick, fValue +' Returns the distance in {distanceUnits} [DDU_MM, DDU_CM, DDU_IN, DDU_FT, DDU_M, DDU_KM, DDU_MI] travelled by this motor since last reset + fMMpTick := float(circInMM_x10) /. 90.0 /. 10.0 + case eDistanceUnits + DDU_MM: + nDistanceInUnits := trunc(float(nValue) *. fMMpTick) + DDU_CM: + nDistanceInUnits := trunc(float(nValue) *. fMMpTick /. 10.0) + DDU_IN: + fValue := distConv.fMm2inFloat(float(nValue) *. fMMpTick) + nDistanceInUnits := trunc(fValue +. 0.5) ' rounded + DDU_FT: + fValue := distConv.fMm2inFloat(float(nValue) *. fMMpTick) + nDistanceInUnits := trunc(fValue /. 12.0 +. 0.5) ' in FT, rounded + DDU_M: + fValue := float(nValue) *. fMMpTick + nDistanceInUnits := trunc(fValue /. 1000.0) ' in M + DDU_MI: + fValue := distConv.fMm2inFloat(float(nValue) *. fMMpTick) + nDistanceInUnits := trunc(fValue /. 5280.0 /. 12.0) ' in MI + DDU_KM: + fValue := float(nValue) *. fMMpTick + nDistanceInUnits := trunc(fValue /. 1000.0 /. 1000.0) ' in kM PUB getRotationCount(eRotationUnits) : leftRotationCount, rightRotationCount '' Returns accumulated {*RotationCount} in {rotationUnits} [DRU_DEGREES, DRU_ROTATIONS], since last reset, for each of the motors. - leftRotationCount := ltWheel.getRotationCount(eRotationUnits) - rightRotationCount := rtWheel.getRotationCount(eRotationUnits) + debug("- grc ", sdec_long(ltPosTrkHallTicks), sdec_long(rtPosTrkHallTicks)) + leftRotationCount := convertRotationCount(ltPosTrkHallTicks, eRotationUnits) + rightRotationCount := convertRotationCount(rtPosTrkHallTicks, eRotationUnits) + +PRI convertRotationCount(nValue, eRotationUnits) : rotationCount +' Returns accumulated {rotationCount} in {rotationUnits} [DRU_DEGREES, DRU_ROTATIONS, or DRU_HALL_TICKS], since last reset, for this motor. + rotationCount := -1 + case eRotationUnits + DRU_HALL_TICKS: + rotationCount := nValue + DRU_DEGREES: + ' degrees = ticks * 4 + rotationCount := nValue * 4 + DRU_ROTATIONS: + ' rotations = ticks / 90 + rotationCount := nValue / 90 PUB getPower() : leftPower, rightPower '' Returns the last specified power value [-100 thru +100] for each of the motors (will be zero if the motor is stopped). @@ -302,6 +356,11 @@ PUB getStatus() : eLeftStatus, eRightStatus eLeftStatus := ltWheel.getStatus() eRightStatus := rtWheel.getStatus() +PRI getDriverState() : eLeftState, eRightState +' Returns status of motor-driver state for each motor: enumerated constants: DCS_* + eLeftState := ltWheel.getDriverState() + eRightState := rtWheel.getDriverState() + PUB getMaxSpeed() : nSpeed '' Returns the last specified {maxSpeed} ' NOTE: they both contain same value so return only right-wheel value @@ -343,7 +402,7 @@ PUB isStarting() : bState CON { -- TASK position tracking -- } - STACK_SIZE_LONGS = 48 + STACK_SIZE_LONGS = 64 ' WARNING! we exceed stack at 48! WINDOW_SIZE = 5 VAR { time values & arrays } @@ -360,6 +419,14 @@ VAR { time values & arrays } VAR { Data for Motor Position Tracking } + long ltPosTrkHallTicks + long rtPosTrkHallTicks + long deltaTicks + long priorLtPos + long priorRtPos + long circInMM_x10 + long tickInMM_x10 + ' -------------------------- long newCntsInSec long ltWinIndex long ltWinEntryCt @@ -367,48 +434,56 @@ VAR { Data for Motor Position Tracking } long rtWinIndex long rtWinEntryCt long rtWindowSum - ' -------------------------- - long ltPosTrkHallTicks - long rtPosTrkHallTicks - long deltaTicks - long priorPos - long circInMM_x10 - long tickInMM_x10 ' Position Tracking vars long motorStopHallTicks long motorStopMSecs +PRI resetAccumLeft() + longfill(@ltCountsWindow, 0, WINDOW_SIZE) ' zero our window accum + ltWinEntryCt := ltWindowSum := ltWinIndex := 0 + ltPosTrkHallTicks := 0 + +PRI resetAccumRight() + longfill(@rtCountsWindow, 0, WINDOW_SIZE) ' zero our window accum + rtWinEntryCt := rtWindowSum := rtWinIndex := 0 + rtPosTrkHallTicks := 0 + +CON + + LEFT_WHEEL = TRUE + RIGHT_WHEEL = FALSE PRI taskPostionSense() | senseStartTicks, posThis8th, fValue, fFps, timeNow, eStopCtr ' TASK: every 1/8 Sec (8Hz) read motor pos and calculate RPM and countOf90ths/Sec - pinlow(TEST_PIN_SNS_LOOP) - pinlow(TEST_PIN_SNS_LP_ACTV) + 'pinlow(TEST_PIN_SNS_LOOP) + 'pinlow(TEST_PIN_SNS_LP_ACTV) eStopCtr := 0 + resetAccumLeft() + resetAccumRight() + priorLtStatus := priorRtStatus := -1 repeat ' if just starting up (or MOTOR FAULT), reset our tracker - if ltWheel.getDriveState() == ltWheel.DCS_FAULTED or senseStartTicks == 0 - longfill(@ltCountsWindow, 0, WINDOW_SIZE) ' zero our window accum - ltWinEntryCt := ltWindowSum := ltWinIndex := 0 - ltPosTrkHallTicks := 0 + if ltWheel.getDriverState() == DCS_FAULTED or ltWheel.getDriverState() == DCS_ESTOP or senseStartTicks == 0 + resetAccumLeft() - if rtWheel.getDriveState() == rtWheel.DCS_FAULTED or senseStartTicks == 0 - longfill(@rtCountsWindow, 0, WINDOW_SIZE) ' zero our window accum - rtWinEntryCt := rtWindowSum := rtWinIndex := 0 - rtPosTrkHallTicks := 0 + if rtWheel.getDriverState() == DCS_FAULTED or rtWheel.getDriverState() == DCS_ESTOP or senseStartTicks == 0 + resetAccumRight() + + showDriveStatesOnChange() senseStartTicks := getct() - pintoggle(TEST_PIN_SNS_LOOP) ' set HI - pintoggle(TEST_PIN_SNS_LP_ACTV) ' start of active side - pintoggle(TEST_PIN_SNS_LOOP) ' set low - start of loop + 'pintoggle(TEST_PIN_SNS_LOOP) ' set HI + 'pintoggle(TEST_PIN_SNS_LP_ACTV) ' start of active side + 'pintoggle(TEST_PIN_SNS_LOOP) ' set low - start of loop ' ------------------------------------------ ' PROCESS left motor ' - newCntsInSec := distanceIn90ths(ltWheel.getRawHallTicks()) ' Read the next sensor value + newCntsInSec := distanceIn90ths(ltWheel.getRawHallTicks(), LEFT_WHEEL) ' Read the next sensor value ltWindowSum -= LONG[@ltCountsWindow][ltWinIndex] ' Remove the oldest entry from the sum long [@ltCountsWindow][ltWinIndex] := newCntsInSec ' place the newest reading into the window ltWindowSum += newCntsInSec ' Add the newest reading to the sum @@ -421,9 +496,9 @@ PRI taskPostionSense() | senseStartTicks, posThis8th, fValue, fFps, timeNow, eSt ltPosTrkHallTicks += posThis8th ' ------------------------------------------ - ' PROCESS left motor + ' PROCESS right motor ' - newCntsInSec := distanceIn90ths(rtWheel.getRawHallTicks()) ' Read the next sensor value + newCntsInSec := distanceIn90ths(rtWheel.getRawHallTicks(), RIGHT_WHEEL) ' Read the next sensor value rtWindowSum -= LONG[@rtCountsWindow][rtWinIndex] ' Remove the oldest entry from the sum long [@rtCountsWindow][rtWinIndex] := newCntsInSec ' place the newest reading into the window rtWindowSum += newCntsInSec ' Add the newest reading to the sum @@ -435,6 +510,9 @@ PRI taskPostionSense() | senseStartTicks, posThis8th, fValue, fFps, timeNow, eSt posThis8th := rtWindowSum / rtWinEntryCt rtPosTrkHallTicks += posThis8th + ' ------------------------------------------ + ' EVALUATE any motor stop requests + ' if user set time/distance to stop then stop if we are past the time or distance if motorStopMSecs > 0 timeNow := getms() @@ -446,8 +524,9 @@ PRI taskPostionSense() | senseStartTicks, posThis8th, fValue, fFps, timeNow, eSt stopMotors() motorStopHallTicks := 0 ' and clear user request - pintoggle(TEST_PIN_SNS_LP_ACTV) ' end of active side + 'pintoggle(TEST_PIN_SNS_LP_ACTV) ' end of active side + ' ------------------------------------------ ' iff driver reports e-stop then clear the emergency stop request ' user will request it again if needed ' NOTE: we leave e_stop set for at least 1/4 second before clearing! @@ -457,6 +536,7 @@ PRI taskPostionSense() | senseStartTicks, posThis8th, fValue, fFps, timeNow, eSt clearEmergency() eStopCtr := 0 + ' keep track of runtime for this sensor loop (for debug, HDMI, etc.) deltaTicks := getct() - senseStartTicks ' code uses about 1_880 ticks. This is little over 9 uS @200MHz clock @@ -471,13 +551,130 @@ PRI isEmergency() : bEmergencyState if bEmergencyState debug("--------WHEEL in EMERGENCY!!--------") +PUB showDriveStates() +'' for TESTING dump curr drive states + reportMotorStatus() + reportDrvStatus() + +PRI reportDrvStatus() | ltState, rtState + ltState, rtState := getDriverState() + showDriverState(ltState, @"ltMot") + showDriverState(rtState, @"rtMot") + +PRI reportMotorStatus() | ltStatus, rtStatus + ltStatus, rtStatus := getStatus() + showStatus(ltStatus, @"ltMot") + showStatus(rtStatus, @"rtMot") + +PRI showDriveStatesOnChange() + reportDrvStatusOnChange() + reportMotorStatusOnChange() + +PRI reportDrvStatusOnChange() | ltState, rtState + ltState, rtState := getDriverState() + if priorLtState <> ltState + showDriverState(ltState, @"ltMot") + priorLtState := ltState + + if priorRtState <> rtState + showDriverState(rtState, @"rtMot") + priorRtState := rtState + +PRI reportMotorStatusOnChange() | ltStatus, rtStatus + ltStatus, rtStatus := getStatus() + if priorLtStatus <> ltStatus + showStatus(ltStatus, @"ltMot") + priorLtStatus := ltStatus + + if priorRtStatus <> rtStatus + showStatus(rtStatus, @"rtMot") + priorRtStatus := rtStatus + +DAT { debug strings } + + dsUnknown BYTE "dsUnk",0 + dsMOVING BYTE "dsMov",0 + dsHOLDING BYTE "dsHld",0 + dsOFF BYTE "dsOff",0 + dsOther BYTE "?ds0x" + dsOtherVal BYTE "00-[CODE]?",0 + + dcsUnknown BYTE "dcsUnk",0 + dcsSTOPPED BYTE "dcsSTOPPED",0 + dcsSPIN_UP BYTE "dcsSPIN_UP",0 + dcsAT_SPEED BYTE "dcsAT_SPEED",0 + dcsSPIN_DN BYTE "dcsSPIN_DN",0 + dcsSLOWING_DN BYTE "dcsSLOWING_DN",0 + dcsSLOW_TO_CHG BYTE "dcsSLOW_TO_CHG",0 + dcsFAULTED BYTE "dcsFAULTED",0 + dcsESTOP BYTE "dcsESTOP",0 + dcsOther BYTE "?dcs0x" + dcsOtherVal BYTE "00-[CODE]?",0 + + pStatVal LONG 0 + priorLtStatus LONG -1 + priorRtStatus LONG -1 + priorLtState LONG -1 + priorRtState LONG -1 + + +PRI showDriverState(eState, pMsg) + case eState + DCS_Unknown: + pStatVal := @dcsUnknown + DCS_STOPPED: + pStatVal := @dcsSTOPPED + DCS_SPIN_UP: + pStatVal := @dcsSPIN_UP + DCS_AT_SPEED: + pStatVal := @dcsAT_SPEED + DCS_SPIN_DN: + pStatVal := @dcsSPIN_DN + DCS_SLOWING_DN: + pStatVal := @dcsSLOWING_DN + DCS_SLOW_TO_CHG: + pStatVal := @dcsSLOW_TO_CHG + DCS_FAULTED: + pStatVal := @dcsFAULTED + DCS_ESTOP: + pStatVal := @dcsESTOP + other: + placeAsciiDigits(@dcsOtherVal, eState) + pStatVal := @dcsOther + debug("- STATE ", zstr_(pMsg), ": ", zstr_(pStatVal)) + +PRI showStatus(eStatus, pMsg) + case eStatus + DS_Unknown: + pStatVal := @dsUnknown + DS_MOVING: + pStatVal := @dsMOVING + DS_HOLDING: + pStatVal := @dsHOLDING + DS_OFF: + pStatVal := @dsOFF + other: + placeAsciiDigits(@dsOtherVal, eStatus) + pStatVal := @dsOther + debug("- STATUS ", zstr_(pMsg), ": ", zstr_(pStatVal)) + +PRI placeAsciiDigits(pStrBytes, byt) | hiNyb, lowNyb + hiNyb := byt & $f0 >> 4 + $30 + if hiNyb > $39 + hiNyb += 7 + lowNyb := byt & $0f + $30 + if lowNyb > $39 + lowNyb += 7 + BYTE [pStrBytes][0] := hiNyb + BYTE [pStrBytes][1] := lowNyb PRI MAX(a,b) : nMax ' return max of a or b nMax := (a > b) ? a : b -PRI distanceIn90ths(newPos) : n90ths +PRI distanceIn90ths(newPos, bIsLeftWheel) : n90ths | priorPos + priorPos := (bIsLeftWheel) ? priorLtPos : priorRtPos if newPos == priorPos n90ths := 0 elseif newPos < 0 @@ -499,7 +696,17 @@ PRI distanceIn90ths(newPos) : n90ths ' 9 -> 6 = 6 - 9 = abs(-3) = 3 ' 9 -> 12 = 12 - 9 = abs(3) = 3 n90ths := abs(newPos - priorPos) - priorPos := newPos +{ + if priorPos <> newPos + if bIsLeftWheel + debug("di90s-LT: ", sdec_long(newPos), sdec_long(priorPos), sdec_long(n90ths)) + else + debug("di90s-RT: ", sdec_long(newPos), sdec_long(priorPos), sdec_long(n90ths)) +'} + if bIsLeftWheel + priorLtPos := newPos + else + priorRtPos := newPos CON { license } diff --git a/isp_steering_serial.spin2 b/isp_steering_serial.spin2 new file mode 100644 index 0000000..dd9c562 --- /dev/null +++ b/isp_steering_serial.spin2 @@ -0,0 +1,521 @@ +'' ================================================================================================= +'' +'' File....... isp_steering_serial.spin2 +'' Purpose.... Top-level Object providing serial control over steering of a twin-bldc-motor platform +'' Authors.... Stephen M Moraco +'' -- Copyright (c) 2022 Iron Sheep Productions, LLC +'' -- see below for terms of use +'' E-mail..... stephen@ironsheep.biz +'' Started.... Apr 2022 +'' Updated.... 4 May 2022 +'' +'' ================================================================================================= + +CON { timing } + + CLK_FREQ = 270_000_000 ' system freq as a constant + _clkfreq = CLK_FREQ ' set system clock + +CON { fixed io pins } + + RX1 = 63 { I } ' programming / debug + TX1 = 62 { O } + + SF_CS = 61 { O } ' serial flash + SF_SCK = 60 { O } + SF_SDO = 59 { O } + SF_SDI = 58 { I } + +CON { application io pins } + + GW_RX2 = 57 { I } ' programming / debug + GW_TX2 = 56 { O } + + GW_BAUDRATE = 624_000 ' 624kb/s - allow P2 rx to keep up! + +DAT { our hardware ID strings and 1-wire buffers, collection names } + +p2HardwareID byte "P2 Edge - 2WheelBot",0 + +OBJ { our Motors } + + wheels : "isp_steering_2wheel" ' our steering object + hostIF : "isp_host_serial" ' serial I/O to/from host (RPi, Arduino, etc.) + serialQueue : "isp_queue_serial" ' access to our received data + strFmt : "isp_mem_strings" ' format to memory routines + +CON { command-names enum } + +#0, CMD_Unknown, CMD_DRV_DIR, CMD_DRV_DIST, CMD_DRV_PWR +#4, CMD_STP_ROT, CMD_STP_DIST, CMD_STP_TIME, CMD_STP_MOT +#8, CMD_EMER_CUT, CMD_EMER_CLR +#10, CMD_SET_ACCEL, CMD_SET_SPD, CMD_SET_DIST_SPD +#13, CMD_HOLD, CMD_RST_TRKG +#15, CMD_GET_DIST, CMD_GET_ROT, CMD_GET_PWR +#18, CMD_GET_STAT, CMD_GET_MAX_SPD, CMD_GET_MAX_DIST_SPD + +DAT { command table } + + ' first our command strings + cmdDrvDir BYTE "drivedir",0 + cmdDrvDist BYTE "drivedist",0 + cmdDrvPwr BYTE "drivepwr",0 + cmdStpAftRot BYTE "stopaftrot",0 + cmdStpAftDist BYTE "stopaftdist",0 + cmdStpAftTime BYTE "stopafttime",0 + cmdStpMot BYTE "stopmotors",0 + cmdEmerCut BYTE "emercutoff",0 + cmdEmerClr BYTE "emerclear",0 + cmdSetAccel BYTE "setaccel",0 + cmdSetSpd BYTE "setspeed",0 + cmdSetDistSpd BYTE "setspeedfordist",0 + cmdHold BYTE "hold",0 + cmdRstTrack BYTE "resettracking",0 + cmdGetDist BYTE "getdist",0 + cmdGetRot BYTE "getrot",0 + cmdGetPwr BYTE "getpwr",0 + cmdGetStat BYTE "getstatus",0 + cmdGetMaxSpd BYTE "getmaxspd",0 + cmdGetMaxDistSpd BYTE "getmaxspdfordist",0 + + ' now describe cmd enum and # parms for each string + cmdsFirst long @cmdDrvDir, CMD_DRV_DIR, 2 + long @cmdDrvDist, CMD_DRV_DIST, 3 + long @cmdDrvPwr, CMD_DRV_PWR, 2 + long @cmdStpAftRot, CMD_STP_ROT, 2 + long @cmdStpAftDist, CMD_STP_DIST, 2 + long @cmdStpAftTime, CMD_STP_TIME, 2 + long @cmdStpMot, CMD_STP_MOT, 0 + long @cmdEmerCut, CMD_EMER_CUT, 0 + long @cmdEmerClr, CMD_EMER_CLR, 0 + long @cmdSetAccel, CMD_SET_ACCEL, 1 + long @cmdSetSpd, CMD_SET_SPD, 1 + long @cmdSetDistSpd, CMD_SET_DIST_SPD, 1 + long @cmdHold, CMD_HOLD, 1 + long @cmdRstTrack, CMD_RST_TRKG, 0 + long @cmdGetDist, CMD_GET_DIST, 1 + long @cmdGetRot, CMD_GET_ROT, 1 + long @cmdGetPwr, CMD_GET_PWR, 0 + long @cmdGetStat, CMD_GET_STAT, 0 + long @cmdGetMaxSpd, CMD_GET_MAX_SPD, 0 + long @cmdGetMaxDistSpd, CMD_GET_MAX_DIST_SPD, 0 + cmdLast + ' calculate the number of entries in table + cmdEntryCt long ((@cmdLast - @cmdsFirst) / 4) / 3 + + + msgWork BYTE 0[256] ' 256 bytes in which to format the outgoing message + + +PUB main() | opStatus, pValues +'' DEMO Driving a two wheeled platform + + debug("* Serial I/F to dual motor platform") + + ' replace original addresses with better values + long[@cmdsFirst][0 * 3] := @cmdDrvDir + long[@cmdsFirst][1 * 3] := @cmdDrvDist + long[@cmdsFirst][2 * 3] := @cmdDrvPwr + long[@cmdsFirst][3 * 3] := @cmdStpAftRot + long[@cmdsFirst][4 * 3] := @cmdStpAftDist + long[@cmdsFirst][5 * 3] := @cmdStpAftTime + long[@cmdsFirst][6 * 3] := @cmdStpMot + long[@cmdsFirst][7 * 3] := @cmdEmerCut + long[@cmdsFirst][8 * 3] := @cmdEmerClr + long[@cmdsFirst][9 * 3] := @cmdSetAccel + long[@cmdsFirst][10 * 3] := @cmdSetSpd + long[@cmdsFirst][11 * 3] := @cmdSetDistSpd + long[@cmdsFirst][12 * 3] := @cmdHold + long[@cmdsFirst][13 * 3] := @cmdRstTrack + long[@cmdsFirst][14 * 3] := @cmdGetDist + long[@cmdsFirst][15 * 3] := @cmdGetRot + long[@cmdsFirst][16 * 3] := @cmdGetPwr + long[@cmdsFirst][17 * 3] := @cmdGetStat + long[@cmdsFirst][18 * 3] := @cmdGetMaxSpd + long[@cmdsFirst][19 * 3] := @cmdGetMaxDistSpd + + ' start our motor drivers (left and right) + wheels.start(wheels.PINS_P0_P15, wheels.PINS_P16_P31, wheels.PWR_14p8V) + + '' start our serial host communications + hostIF.startx(GW_RX2, GW_TX2, GW_BAUDRATE, hostIF.PU_15K) ' tell singleton our pins and rate + + ' (one time) tell the RPi about how to identify this hardware + hostIF.identify(@p2HardwareID) + + ' override defaults, use 100 % + 'wheels.setMaxSpeed(100) + 'wheels.setMaxSpeedForDistance(100) + ' just don't draw current at stop + 'wheels.holdAtStop(false) + + debug("* command loop *") + + repeat + if serialQueue.haveCommand(@cmdsFirst, cmdEntryCt) + opStatus, pValues := serialQueue.getCommandParms() + if opStatus == serialQueue.OP_SUCCESS + processCommand(pValues) + else + waitms(1000) ' for 1 sec + +DAT { parse work variables } + + cmdEnum LONG 0 + drvDir LONG 0 + drvPwr1 LONG 0 + drvPwr2 LONG 0 + onlyValue LONG 0 + ltValue LONG 0 + rtValue LONG 0 + nRate LONG 0 + nRot LONG 0 + nRotUnits LONG 0 + nTime LONG 0 + nTimeUnits LONG 0 + nDist LONG 0 + nDistUnits LONG 0 + nSpeed LONG 0 + bHoldEnable LONG 0 + +PRI processCommand(pValues) + debug("* HANDLE ", zstr_(long [pValues][serialQueue.ENT_CMDSTR_IDX])) +' pValues -> [0:nbrValues(1-n)] (doesn't count "full string") ENT_VALUE_CT_IDX +' [1:pStr] -> "full string" ENT_RAWSTR_IDX +' [2:pCmd] -> "cmd" ENT_CMDSTR_IDX +' [3:eCmd] ENT_CMDENUM_IDX +' [4:nValue1] ENT_PARM1_IDX +' [5:nValue2] ENT_PARM2_IDX +' [6:nValue3] ENT_PARM3_IDX +' [7:nValue4] ENT_PARM4_IDX +' [etc] - up to 6 max values + cmdEnum := long [pValues][serialQueue.ENT_CMDENUM_IDX] + case cmdEnum + CMD_DRV_DIR: + ' get incoming parm values + drvPwr1 := LONG[pValues][serialQueue.ENT_PARM1_IDX] + drvDir := LONG[pValues][serialQueue.ENT_PARM2_IDX] + ' validate values + if not inRange(drvPwr1, -100, 100) + ' invalid, report so... + strFmt.sFormatStr1(@msgWork, @"Power (%d) out of range [-100, 100]", drvPwr1) + serialQueue.sendError(@msgWork) + elseif not inRange(drvDir, -100, 100) + ' invalid, report so... + strFmt.sFormatStr1(@msgWork, @"Direction (%d) out of range [-100, 100]", drvDir) + serialQueue.sendError(@msgWork) + else + ' all good, say so, then act on request + serialQueue.sendOK() + wheels.driveDirection(drvPwr1, drvDir) + + CMD_DRV_DIST: + ' get incoming parm values + ltValue := LONG[pValues][serialQueue.ENT_PARM1_IDX] + rtValue := LONG[pValues][serialQueue.ENT_PARM2_IDX] + nDistUnits := LONG[pValues][serialQueue.ENT_PARM3_IDX] + ' validate values + if not isPostiveValue(ltValue) + ' invalid, report so... + strFmt.sFormatStr1(@msgWork, @"LT-distance (%d) must be positive value", ltValue) + serialQueue.sendError(@msgWork) + elseif not isPostiveValue(rtValue) + ' invalid, report so... + strFmt.sFormatStr1(@msgWork, @"RT-distance (%d) must be positive value", rtValue) + serialQueue.sendError(@msgWork) + elseif not isValidDistanceUnit(nDistUnits, @msgWork) ' fills in msgWork with error msg + ' invalid, report so... + serialQueue.sendError(@msgWork) + else + ' all good, say so, then act on request + serialQueue.sendOK() + wheels.driveForDistance(ltValue, rtValue, nDistUnits) + + CMD_DRV_PWR: + ' get incoming parm values + drvPwr1 := LONG[pValues][serialQueue.ENT_PARM1_IDX] + drvPwr2 := LONG[pValues][serialQueue.ENT_PARM2_IDX] + ' validate values + if not inRange(drvPwr1, -100, 100) + ' invalid, report so... + strFmt.sFormatStr1(@msgWork, @"LT-Power (%d) out of range [-100, 100]", drvPwr1) + serialQueue.sendError(@msgWork) + elseif not inRange(drvPwr2, -100, 100) + ' invalid, report so... + strFmt.sFormatStr1(@msgWork, @"RT-Power (%d) out of range [-100, 100]", drvPwr2) + serialQueue.sendError(@msgWork) + else + ' all good, say so, then act on request + serialQueue.sendOK() + wheels.driveAtPower(drvPwr1, drvPwr2) + + CMD_STP_ROT: + ' get incoming parm values + nRot := LONG[pValues][serialQueue.ENT_PARM1_IDX] + nRotUnits := LONG[pValues][serialQueue.ENT_PARM2_IDX] + ' validate values + if not isValidRotationUnit(nRotUnits, @msgWork) ' fills in msgWork with error msg + ' invalid, report so... + serialQueue.sendError(@msgWork) + elseif not isPostiveValue(nRot) + ' invalid, report so... + strFmt.sFormatStr1(@msgWork, @"Rotation Count (%d) must be positive value", nRot) + serialQueue.sendError(@msgWork) + else + ' all good, say so, then act on request + serialQueue.sendOK() + wheels.stopAfterRotation(nRot, nRotUnits) + + CMD_STP_DIST: + ' get incoming parm values + nDist := LONG[pValues][serialQueue.ENT_PARM1_IDX] + nDistUnits := LONG[pValues][serialQueue.ENT_PARM2_IDX] + ' validate values + if not isValidDistanceUnit(nDistUnits, @msgWork) ' fills in msgWork with error msg + ' invalid, report so... + serialQueue.sendError(@msgWork) + elseif not isPostiveValue(nDist) + ' invalid, report so... + strFmt.sFormatStr1(@msgWork, @"Distance Value (%d) must be positive", nDist) + serialQueue.sendError(@msgWork) + else + ' all good, say so, then act on request + serialQueue.sendOK() + wheels.stopAfterDistance(nDist, nDistUnits) + + CMD_STP_TIME: + ' get incoming parm values + nTime := LONG[pValues][serialQueue.ENT_PARM1_IDX] + nTimeUnits := LONG[pValues][serialQueue.ENT_PARM2_IDX] + ' validate values + if not isValidTimeUnit(nTimeUnits, @msgWork) ' fills in msgWork with error msg + ' invalid, report so... + serialQueue.sendError(@msgWork) + elseif not isPostiveValue(nTime) + ' invalid, report so... + strFmt.sFormatStr1(@msgWork, @"Time Value (%d) must be positive", nTime) + serialQueue.sendError(@msgWork) + else + ' all good, say so, then act on request + serialQueue.sendOK() + wheels.stopAfterTime(nTime, nTimeUnits) + + CMD_STP_MOT: + ' all good, say so, then act on request + serialQueue.sendOK() + wheels.stopMotors() + + CMD_EMER_CUT: + ' all good, say so, then act on request + ' NOTE: in this case we act first then send response! (best we can do ) + wheels.emergencyCutoff() + serialQueue.sendOK() + + CMD_EMER_CLR: + ' all good, say so, then act on request + serialQueue.sendOK() + wheels.clearEmergency() + + CMD_SET_ACCEL: + ' NOTE: this is not yet functional, say so + strFmt.sFormatStr0(@msgWork, @"setAcceleration(rate) is NOT yet supported") + serialQueue.sendError(@msgWork) + { + ' get incoming parm values + nRate := LONG[pValues][serialQueue.ENT_PARM1_IDX] + ' validate values + if not inRange(nRate, ??, ??) + ' invalid, report so... + strFmt.sFormatStr1(@msgWork, @"Accel Rate (%d) out of range [??, ??]", nRate) + serialQueue.sendError(@msgWork) + else + ' all good, say so, then act on request + serialQueue.sendOK() + wheels.setAcceleration(nRate) + '} + + CMD_SET_SPD: + ' get incoming parm values + nSpeed := LONG[pValues][serialQueue.ENT_PARM1_IDX] + ' validate values + if not inRange(nSpeed, 1, 100) + ' invalid, report so... + strFmt.sFormatStr1(@msgWork, @"MaxSpeed (%d) out of range [1, 100]", nSpeed) + serialQueue.sendError(@msgWork) + else + ' all good, say so, then act on request + serialQueue.sendOK() + wheels.setMaxSpeed(nSpeed) + + CMD_SET_DIST_SPD: + ' get incoming parm values + nSpeed := LONG[pValues][serialQueue.ENT_PARM1_IDX] + ' validate values + if not inRange(nSpeed, 1, 100) + ' invalid, report so... + strFmt.sFormatStr1(@msgWork, @"MaxSpeedForDistance (%d) out of range [1, 100]", nSpeed) + serialQueue.sendError(@msgWork) + else + ' all good, say so, then act on request + serialQueue.sendOK() + wheels.setMaxSpeedForDistance(nSpeed) + + CMD_HOLD: + ' get incoming parm values + bHoldEnable := LONG[pValues][serialQueue.ENT_PARM1_IDX] + ' validate values + if not (bHoldEnable == TRUE or bHoldEnable == FALSE) + ' invalid, report so... + strFmt.sFormatStr1(@msgWork, @"HoldAtStop (%d) out of range [-1, 0] (T/F)", bHoldEnable) + serialQueue.sendError(@msgWork) + else + ' all good, say so, then act on request + serialQueue.sendOK() + wheels.holdAtStop(bHoldEnable) + + CMD_RST_TRKG: + ' all good, say so, then act on request + serialQueue.sendOK() + wheels.resetTracking() + + CMD_GET_DIST: + ' get incoming parm values + nDistUnits := LONG[pValues][serialQueue.ENT_PARM1_IDX] + ' validate values + if not isValidDistanceUnit(nDistUnits, @msgWork) ' fills in msgWork with error msg + ' invalid, report so... + serialQueue.sendError(@msgWork) + else + ' get requested value(s) + ltValue, rtValue := wheels.getDistance(nDistUnits) + ' format response + strFmt.sFormatStr2(@msgWork, @"dist %d %d\n", ltValue, rtValue) + ' send to host + serialQueue.sendResponse(@msgWork) + + CMD_GET_ROT: + ' get incoming parm values + nRotUnits := LONG[pValues][serialQueue.ENT_PARM1_IDX] + ' validate values + if not isValidRotationUnit(nRotUnits, @msgWork) ' fills in msgWork with error msg + ' invalid, report so... + serialQueue.sendError(@msgWork) + else + ' get requested value(s) + ltValue, rtValue := wheels.getRotationCount(nRotUnits) + ' format response + strFmt.sFormatStr2(@msgWork, @"rot %d %d\n", ltValue, rtValue) + ' send to host + serialQueue.sendResponse(@msgWork) + + CMD_GET_PWR: + ' get requested value(s) + ltValue, rtValue := wheels.getPower() + ' format response + strFmt.sFormatStr2(@msgWork, @"pwr %d %d\n", ltValue, rtValue) + ' send to host + serialQueue.sendResponse(@msgWork) + + CMD_GET_STAT: + ' get requested value(s) + ltValue, rtValue := wheels.getStatus() + ' format response + strFmt.sFormatStr2(@msgWork, @"stat %d %d\n", ltValue, rtValue) + ' send to host + serialQueue.sendResponse(@msgWork) + + CMD_GET_MAX_SPD: + ' get requested value(s) + onlyValue := wheels.getMaxSpeed() + ' format response + strFmt.sFormatStr1(@msgWork, @"speedmax %d\n", onlyValue) + ' send to host + serialQueue.sendResponse(@msgWork) + + CMD_GET_MAX_DIST_SPD: + ' get requested value(s) + onlyValue := wheels.getMaxSpeedForDistance() + ' format response + strFmt.sFormatStr1(@msgWork, @"speeddistmax %d\n", onlyValue) + ' send to host + serialQueue.sendResponse(@msgWork) + + other: + debug("ERROR bad ENUM for command:", udec_(cmdEnum)) + +PRI inRange(value, min, max) : bInRangeStatus + bInRangeStatus := TRUE + if value < min or value > max + bInRangeStatus := FALSE + +PRI isPostiveValue(value) : bPositiveStatus + bPositiveStatus := TRUE + if value < 0 + bPositiveStatus := FALSE + +PRI isValidTimeUnit(timeUnits, pMsgBffr) : bValidStatus +' validate and return message when invalid + bValidStatus := TRUE + case timeUnits + wheels.DTU_MILLISEC: + wheels.DTU_SEC: + other: + bValidStatus := FALSE + if pMsgBffr <> 0 + strFmt.sFormatStr1(pMsgBffr, @"TimeUnits (%d) not [DTU_MILLISEC, DTU_SEC]", timeUnits) + +PRI isValidRotationUnit(rotUnits, pMsgBffr) : bValidStatus +' validate and return message when invalid + bValidStatus := TRUE + case rotUnits + wheels.DRU_DEGREES: + wheels.DRU_ROTATIONS: + wheels.DRU_HALL_TICKS: + other: + bValidStatus := FALSE + if pMsgBffr <> 0 + strFmt.sFormatStr1(pMsgBffr, @"RotationUnits (%d) not [DRU_DEGREES, DRU_ROTATIONS, DRU_HALL_TICKS]", rotUnits) + +PRI isValidDistanceUnit(distUnits, pMsgBffr) : bValidStatus +' validate and return message when invalid + bValidStatus := TRUE + case distUnits + wheels.DDU_IN: + wheels.DDU_MM: + wheels.DDU_CM: + wheels.DDU_FT: + wheels.DDU_M: + wheels.DDU_KM: + wheels.DDU_MI: + other: + bValidStatus := FALSE + if pMsgBffr <> 0 + strFmt.sFormatStr1(pMsgBffr, @"DistanceUnits (%d) not [DDU_IN, DDU_MM, DDU_CM, DDU_FT, DDU_M, DDU_KM, DDU_MI]", distUnits) + +CON { license } +{{ + + ------------------------------------------------------------------------------------------------- + MIT License + + Copyright (c) 2022 Iron Sheep Productions, LLC + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + ================================================================================================= +}} diff --git a/pythonSrc/P2-BLDC-Motor-Control-Demo.py b/pythonSrc/P2-BLDC-Motor-Control-Demo.py new file mode 100755 index 0000000..ba1d630 --- /dev/null +++ b/pythonSrc/P2-BLDC-Motor-Control-Demo.py @@ -0,0 +1,771 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import _thread +from datetime import datetime +from math import e +from pickle import TRUE +from time import time, sleep, localtime, strftime +import os +import subprocess +import sys +import os.path +import json +import argparse +from collections import deque +from pkg_resources import UnknownExtra +from unidecode import unidecode +from colorama import init as colorama_init +from colorama import Fore, Back, Style +import serial +from time import sleep +from configparser import ConfigParser +from email.mime.text import MIMEText +from subprocess import Popen, PIPE +import sendgrid +from sendgrid.helpers.mail import Content, Email, Mail +from enum import Enum, unique +from signal import signal, SIGPIPE, SIG_DFL +signal(SIGPIPE,SIG_DFL) +from watchdog.observers import Observer +from watchdog.events import FileSystemEventHandler + +if False: + # will be caught by python 2.7 to be illegal syntax + print_line('Sorry, this script requires a python3 runtime environment.', file=sys.stderr) + os._exit(1) + +script_version = "1.0.0" +script_name = 'P2-BLDC-Motor-Control-Demo.py' +script_info = '{} v{}'.format(script_name, script_version) +project_name = 'P2-BLDC-Motor-Control' +project_url = 'https://github.com/ironsheep/P2-BLDC-Motor-Control' + +# ----------------------------------------------------------------------------- +# the BELOW are identical to that found in our gateway .spin2 object +# (!!!they must be kept in sync!!!) +# ----------------------------------------------------------------------------- + +# markers found within the data arriving from the P2 but likely will NOT be found in normal user data sent by the P2 +parm_sep = '^|^' + +# ----------------------------------------------------------------------------- +# External Interface constants (Enums) exposed by isp_bldc_motor.spin2 +# REF: https://github.com/ironsheep/P2-BLDC-Motor-Control/blob/develop/isp_bldc_motor.spin2 +# ----------------------------------------------------------------------------- +""" + ' Driver Distance-Units Enum: (Millimeters, Centimeters, Inches, Feet, Meters, Kilometers, Miles) + #0, DDU_Unknown, DDU_MM, DDU_CM, DDU_IN, DDU_FT, DDU_M, DDU_KM, DDU_MI + + ' Driver Rotation-Units Enum: + #0, DRU_Unknown, DRU_HALL_TICKS, DRU_DEGREES, DRU_ROTATIONS + + ' Driver Time-Unit Enum: + #0, DTU_Unknown, DTU_MILLISEC, DTU_SEC + + ' Driver Status Enum: + #10, DS_Unknown, DS_MOVING, DS_HOLDING, DS_OFF + + ' Driver Control Stop-State Enum: + #0, SM_Unknown, SM_FLOAT, SM_BRAKE +""" + +# the following enum name orders and starting values must be identical to that +# found in our isp_bldc_motor.spin2 object +DrvDistUnits = Enum('DrvDistUnits', [ + 'DDU_Unknown', + 'DDU_MM', + 'DDU_CM', + 'DDU_IN', + 'DDU_FT', + 'DDU_M', + 'DDU_KM', + 'DDU_MI'], start=0) + +DrvRotUnits = Enum('DrvRotUnits', [ + 'DRU_Unknown', + 'DRU_HALL_TICKS', + 'DRU_DEGREES', + 'DRU_ROTATIONS'], start=0) + +DrvTimeUnits = Enum('DrvTimeUnits', [ + 'DTU_Unknown', + 'DTU_MILLISEC', + 'DTU_SECS'], start=0) + +DrvStatus = Enum('DrvStatus', [ + 'DS_Unknown', + 'DS_MOVING', + 'DS_HOLDING', + 'DS_OFF'], start=10) + +DrvStopState = Enum('DrvStopState', [ + 'SM_Unknown', + 'SM_FLOAT', + 'SM_BRAKE'], start=0) + + +# Colorama constants: +# Fore: BLACK, RED, GREEN, YELLOW, BLUE, MAGENTA, CYAN, WHITE, RESET. +# Back: BLACK, RED, GREEN, YELLOW, BLUE, MAGENTA, CYAN, WHITE, RESET. +# Style: DIM, NORMAL, BRIGHT, RESET_ALL +# +# Logging function +def print_line(text, error=False, warning=False, info=False, verbose=False, debug=False, console=True): + timestamp = strftime('%Y-%m-%d %H:%M:%S', localtime()) + if console: + if error: + print(Fore.RED + Style.BRIGHT + '[{}] '.format(timestamp) + Style.NORMAL + '{}'.format(text) + Style.RESET_ALL, file=sys.stderr) + elif warning: + print(Fore.YELLOW + Style.BRIGHT + '[{}] '.format(timestamp) + Style.NORMAL + '{}'.format(text) + Style.RESET_ALL) + elif info or verbose: + if verbose: + # conditional verbose output... + if opt_verbose: + print(Fore.GREEN + '[{}] '.format(timestamp) + Fore.YELLOW + '- ' + '{}'.format(text) + Style.RESET_ALL) + else: + # info... + print(Fore.MAGENTA + '[{}] '.format(timestamp) + Fore.WHITE + '- ' + '{}'.format(text) + Style.RESET_ALL) + elif debug: + # conditional debug output... + if opt_debug: + print(Fore.CYAN + '[{}] '.format(timestamp) + '- (DBG): ' + '{}'.format(text) + Style.RESET_ALL) + else: + print(Fore.GREEN + '[{}] '.format(timestamp) + Style.RESET_ALL + '{}'.format(text) + Style.RESET_ALL) + +# ----------------------------------------------------------------------------- +# Script Argument parsing +# ----------------------------------------------------------------------------- + +# Argparse +opt_debug = False +opt_verbose = False +opt_useTestFile = False + +# Argparse +parser = argparse.ArgumentParser(description=project_name, epilog='For further details see: ' + project_url) +parser.add_argument("-v", "--verbose", help="increase output verbosity", action="store_true") +parser.add_argument("-d", "--debug", help="show debug output", action="store_true") +parse_args = parser.parse_args() + +opt_verbose = parse_args.verbose +opt_debug = parse_args.debug + +print_line(script_info, info=True) +if opt_verbose: + print_line('Verbose enabled', verbose=True) +if opt_debug: + print_line('Debug enabled', debug=True) + + +# ----------------------------------------------------------------------------- +# CLASS methods indentifying RPi host hardware/software +# ----------------------------------------------------------------------------- + +# object that provides access to information about the RPi on which we are running +class RPiHostInfo: + + def getDeviceModel(self): + out = subprocess.Popen("/bin/cat /proc/device-tree/model | /bin/sed -e 's/\\x0//g'", + shell=True, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT) + stdout, _ = out.communicate() + model_raw = stdout.decode('utf-8') + # now reduce string length (just more compact, same info) + model = model_raw.replace('Raspberry ', 'R').replace( + 'i Model ', 'i 1 Model').replace('Rev ', 'r').replace(' Plus ', '+ ') + + print_line('rpi_model_raw=[{}]'.format(model_raw), debug=True) + print_line('rpi_model=[{}]'.format(model), debug=True) + return model, model_raw + + def getLinuxRelease(self): + out = subprocess.Popen("/bin/cat /etc/apt/sources.list | /bin/egrep -v '#' | /usr/bin/awk '{ print $3 }' | /bin/grep . | /usr/bin/sort -u", + shell=True, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT) + stdout, _ = out.communicate() + linux_release = stdout.decode('utf-8').rstrip() + print_line('rpi_linux_release=[{}]'.format(linux_release), debug=True) + return linux_release + + + def getLinuxVersion(self): + out = subprocess.Popen("/bin/uname -r", + shell=True, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT) + stdout, _ = out.communicate() + linux_version = stdout.decode('utf-8').rstrip() + print_line('rpi_linux_version=[{}]'.format(linux_version), debug=True) + return linux_version + + + def getHostnames(self): + out = subprocess.Popen("/bin/hostname -f", + shell=True, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT) + stdout, _ = out.communicate() + fqdn_raw = stdout.decode('utf-8').rstrip() + print_line('fqdn_raw=[{}]'.format(fqdn_raw), debug=True) + lcl_hostname = fqdn_raw + if '.' in fqdn_raw: + # have good fqdn + nameParts = fqdn_raw.split('.') + lcl_fqdn = fqdn_raw + tmpHostname = nameParts[0] + else: + # missing domain, if we have a fallback apply it + if len(fallback_domain) > 0: + lcl_fqdn = '{}.{}'.format(fqdn_raw, fallback_domain) + else: + lcl_fqdn = lcl_hostname + + print_line('rpi_fqdn=[{}]'.format(lcl_fqdn), debug=True) + print_line('rpi_hostname=[{}]'.format(lcl_hostname), debug=True) + return lcl_hostname, lcl_fqdn + +# ----------------------------------------------------------------------------- +# CLASS: Maintain Runtime Configuration values +# ----------------------------------------------------------------------------- + +# object that provides access to gateway runtime confiration data +class RuntimeConfig: + # Host RPi keys + keyRPiModel = "Model" + keyRPiMdlFull = "ModelFull" + keyRPiRel = "OsRelease" + keyRPiVer = "OsVersion" + keyRPiName = "Hostname" + keyRPiFqdn = "FQDN" + + # P2 Hardware/Application keys + keyHwName = "hwName" + keyObjVer = "objVer" + + # searchable list of keys + configKnownKeys = [ keyHwName, keyObjVer, + keyRPiModel, keyRPiMdlFull, keyRPiRel, keyRPiVer, keyRPiName, keyRPiFqdn ] + + configDictionary = {} # initially empty + + def validateKey(self, name): + # ensure a key we are trying to set/get is expect by this system + # generate warning if NOT + if name not in self.configKnownKeys: + print_line('CONFIG-Dict: Unexpected key=[{}]!!'.format(name), warning=True) + + def containsKey(self, possKey): + # ensure a key we are trying to set/get is expect by this system + # generate warning if NOT + foundKeyStatus = False + if possKey in self.configDictionary: + foundKeyStatus = True + return foundKeyStatus + + def setConfigNamedVarValue(self, name, value): + # set a config value for name + global configDictionary + self.validateKey(name) # warn if key isn't a know key + foundKey = False + if name in self.configDictionary.keys(): + oldValue = self.configDictionary[name] + foundKey = True + self.configDictionary[name] = value + if foundKey and oldValue != value: + print_line('CONFIG-Dict: [{}]=[{}]->[{}]'.format(name, oldValue, value), debug=True) + else: + print_line('CONFIG-Dict: [{}]=[{}]'.format(name, value), debug=True) + + def getValueForConfigVar(self, name): + # return a config value for name + # print_line('CONFIG-Dict: get({})'.format(name), debug=True) + self.validateKey(name) # warn if key isn't a know key + dictValue = "" + if name in self.configDictionary.keys(): + dictValue = self.configDictionary[name] + print_line('CONFIG-Dict: [{}]=[{}]'.format(name, dictValue), debug=True) + else: + print_line('CONFIG-Dict: [{}] NOT FOUND'.format(name, dictValue), warning=True) + return dictValue + + +# ----------------------------------------------------------------------------- +# CLASS: Circular queue for serial input lines & serial listener +# ----------------------------------------------------------------------------- + +# object which is a queue of text lines +# these arrive at a rate different from our handling them rate +# so we put them in a queue while they wait to be handled +class RxLineQueue: + + lineBuffer = deque() + + def pushLine(self, newLine): + self.lineBuffer.append(newLine) + # show debug every 100 lines more added + if len(self.lineBuffer) % 100 == 0: + print_line('- lines({})'.format(len(self.lineBuffer)),debug=True) + + def popLine(self): + oldestLine = '' + if len(self.lineBuffer) > 0: + oldestLine = self.lineBuffer.popleft() + return oldestLine + + def lineCount(self): + return len(self.lineBuffer) + +# ----------------------------------------------------------------------------- +# CLASS: An interface for easy BLDC Motor control (over serial I/F) +# ----------------------------------------------------------------------------- + +class BLDCMotorControl: + serPort = '' + + # create a new instance with the given serial port + def __init__(self, serialPort): + self.serPort = serialPort + + # ----------------------- + # PUBLIC Control Methods + # ----------------------- + # PUB driveDirection(power, direction) + def driveDirection(self, power, direction): + commandStr = 'drivedir {} {}\n'.format(power, direction) + self.sendCommand(commandStr) + + # PUB driveForDistance(leftDistance, rightDistance, distanceUnits) + def driveForDistance(self, leftDistance, rightDistance, eDistanceUnits): + commandStr = 'drivedist {} {} {}\n'.format(leftDistance, rightDistance, eDistanceUnits.value) + self.sendCommand(commandStr) + + # PUB driveAtPower(leftPower, rightPower) + def driveAtPower(self, leftPower, rightPower): + commandStr = 'drivepwr {} {}\n'.format(leftPower, rightPower) + self.sendCommand(commandStr) + + # PUB stopAfterRotation(rotationCount, rotationUnits) + def stopAfterRotation(self, rotationCount, eRotationUnits): + commandStr = 'stopaftrot {} {}\n'.format(rotationCount, eRotationUnits.value) + self.sendCommand(commandStr) + + # PUB stopAfterDistance(distance, distanceUnits) + def stopAfterDistance(self, distance, eDistanceUnits): + commandStr = 'stopaftdist {} {}\n'.format(distance, eDistanceUnits.value) + self.sendCommand(commandStr) + + # PUB stopAfterTime(time, timeUnits) + def stopAfterTime(self, time, eTimeUnits): + commandStr = 'stopafttime {} {}\n'.format(time, eTimeUnits.value) + self.sendCommand(commandStr) + + # PUB stopMotors() + def stopMotors(self): + commandStr = 'stopmotors\n' + self.sendCommand(commandStr) + + # PUB emergencyCutoff() + def emergencyCutoff(self): + commandStr = 'emercutoff\n' + self.sendCommand(commandStr) + + # PUB clearEmergency() + def clearEmergency(self): + commandStr = 'emerclear\n' + self.sendCommand(commandStr) + + # ------------------------- + # PUBLIC Configure Methods + # ------------------------- + # PUB setAcceleration(rate) + def setAcceleration(self, rate): + commandStr = 'setaccel {}\n'.format(rate) + self.sendCommand(commandStr) + + # PUB setMaxSpeed(speed) + def setMaxSpeed(self, speed): + commandStr = 'setspeed {}\n'.format(speed) + self.sendCommand(commandStr) + + # PUB setMaxSpeedForDistance(speed) + def setMaxSpeedForDistance(self, speed): + commandStr = 'setspeedfordist {}\n'.format(speed) + self.sendCommand(commandStr) + + # PUB holdAtStop(bEnable) + def holdAtStop(self, bEnable): + commandStr = 'hold {}\n'.format(bEnable) + self.sendCommand(commandStr) + + # PUB resetTracking() + def resetTracking(self): + commandStr = 'resettracking\n' + self.sendCommand(commandStr) + + # ---------------------- + # PUBLIC Status Methods + # ---------------------- + # PUB getDistance(distanceUnits) : leftDistanceInUnits, rightDistanceInUnits + def getDistance(self, eDistanceUnits): + commandStr = 'getdist {}\n'.format(eDistanceUnits.value) + responseStr = self.sendCommand(commandStr) + ltValue, rtValue = self.getValues('dist', responseStr, 2) + return ltValue, rtValue + + # PUB getRotationCount(rotationUnits) : leftRotationCount, rightRotationCount + def getRotationCount(self, eRotationUnits) : + commandStr = 'getrot {}\n'.format(eRotationUnits.value) + responseStr = self.sendCommand(commandStr) + ltValue, rtValue = self.getValues('rot', responseStr, 2) + return int(ltValue), int(rtValue) + + # PUB getPower() : leftPower, rightPower + def getPower(self): + commandStr = 'getpwr\n' + responseStr = self.sendCommand(commandStr) + ltValue, rtValue = self.getValues('pwr', responseStr, 2) + return ltValue, rtValue + + # PUB getStatus() : eLeftStatus, eRightStatus + def getStatus(self): + commandStr = 'getstatus\n' + responseStr = self.sendCommand(commandStr) + ltValue, rtValue = self.getValues('stat', responseStr, 2) + return self.statusEnumFor(ltValue), self.statusEnumFor(rtValue) + + # PUB getMaxSpeed() : maxSpeed + def getMaxSpeed(self): + commandStr = 'getmaxspd\n' + responseStr = self.sendCommand(commandStr) + onlyValue = self.getValues('speedmax', responseStr, 1) + return onlyValue + + # PUB getMaxSpeedForDistance() : maxSpeed4dist + def getMaxSpeedForDistance(self): + commandStr = 'getmaxspdfordist\n' + responseStr = self.sendCommand(commandStr) + onlyValue = self.getValues('speeddistmax', responseStr, 1) + return onlyValue + + # ------- PRIVATE (Support) Methods -------- + # common send method + def sendCommand(self, cmdStr): + # format and send command, then wait for single line response + # if other than error|OK then return the response string + newOutLine = cmdStr.encode('utf-8') + print_line('send line({})=({})'.format(len(newOutLine), newOutLine), verbose=True) + self.serPort.write(newOutLine) + responseStr = self.processResponse() + return responseStr + + # common rx repsonse method + def processResponse(self): + # wait for 1-line response + # show debug/error if OK|error + # otherwise return the response string + global queueRxLines + responseStr = '' + while queueRxLines.lineCount() == 0: + sleep(0.2) # wait for 2/10th of second before asking again + + currLine = queueRxLines.popLine().replace('\\n', '') + if currLine.startswith(responseOK): + print_line('Incoming OK', debug=True) + + elif currLine.startswith(responseERROR): + print_line('! {}'.format(currLine), error=True) + else: + responseStr = currLine + return responseStr; + + def getValues(self, linePrefix, line, ctExpected): + # given the response string, isolate and return the response values + lineParts = line.split(' ') + if len(lineParts) < 2: + print_line('! ERROR: not enough parts in line={}'.format(line.replace('\\n', '')), error=True) + elif lineParts[0] != linePrefix: + print_line('! ERROR: BAD line={} missing prefix [{}]'.format(line.replace('\\n', ''), linePrefix), error=True) + elif len(lineParts) != ctExpected + 1: + print_line('! ERROR: BAD line={} wrong number reponses, expected ({})'.format(line.replace('\\n', ''), ctExpected), error=True) + else: + if len(lineParts) == 3: + return lineParts[1], lineParts[2] + else: + return lineParts[1] + + def statusEnumFor(self, iValue): + # return enum member assoc with int value + desiredValue = DrvStatus.DS_Unknown + if int(iValue) in DrvStatus._value2member_map_: + desiredValue = DrvStatus(int(iValue)) + #print_line('found ENUM {} for {}'.format(desiredValue, iValue), debug=True) + return desiredValue + +# ----------------------------------------------------------------------------- +# TASK: dedicated serial listener +# ----------------------------------------------------------------------------- + +def taskSerialListener(serPort): + global queueRxLines + print_line('Thread: taskSerialListener() started', verbose=True) + # process lies from serial or from test file + if opt_useTestFile == True: + test_file=open("charlie_rpi_debug.out", "r") + lines = test_file.readlines() + for currLine in lines: + queueRxLines.pushLine(currLine) + #sleep(0.1) + else: + # queue lines received as quickly as they come in... + while True: + received_data = serPort.readline() #read serial port + if len(received_data) > 0: + print_line('TASK-RX rxD({})=({})'.format(len(received_data),received_data), debug=True) + currLine = received_data.decode('utf-8', 'replace').rstrip() + #print_line('TASK-RX line({}=[{}]'.format(len(currLine), currLine), debug=True) + if currLine != '\\n': # skip blank lines... + queueRxLines.pushLine(currLine) + if currLine.startswith(cmdIdentifyHW): + processInput(serPort) + else: + print_line('TASK-RX skip empty-line({}=[{}]'.format(len(currLine), currLine), warning=True) + + +# commands from P2 +cmdIdentifyHW = "ident:" +responseOK = "OK" +responseERROR = "ERROR" + +def processIncomingRequest(newLine, serPort): + + print_line('Incoming line({})=[{}]'.format(len(newLine), newLine), debug=True) + + if newLine.startswith(responseOK): + print_line('Incoming OK', debug=True) + + elif newLine.startswith(responseERROR): + print_line('Incoming errStr({})=({})'.format(len(newLine), newLine), error=True) + + elif newLine.startswith(cmdIdentifyHW): + print_line('* HANDLE id P2 Hardware', verbose=True) + nameValuePairs = getNameValuePairs(newLine, cmdIdentifyHW) + if len(nameValuePairs) > 0: + findingsDict = processNameValuePairs(nameValuePairs) + # Record the hardware info for later use + if len(findingsDict) > 0: + p2ProcDict = {} + for key in findingsDict: + runtimeConfig.setConfigNamedVarValue(key, findingsDict[key]) + p2ProcDict[key] = findingsDict[key] + sendValidationSuccess(serPort, "fident", "", "") + else: + print_line('processIncomingRequest nameValueStr({})=({}) ! missing hardware keys !'.format(len(newLine), newLine), warning=True) + + else: + print_line('ERROR: line({})=[{}] ! P2 LINE NOT Recognized !'.format(len(newLine), newLine), error=True) + + +def processInput(serPort): + # process all queued lines then stop + global queueRxLines + while True: + # process an incoming line + currLine = queueRxLines.popLine() + + if len(currLine) > 0: + processIncomingRequest(currLine, serPort) + else: + # if no more lines, exit loop + break + +def getNameValuePairs(strRequest, cmdStr): + # isolate name-value pairs found within {strRequest} (after removing prefix {cmdStr}) + rmdr = strRequest.replace(cmdStr,'') + nameValuePairs = rmdr.split(parm_sep) + print_line('getNameValuePairs nameValuePairs({})=({})'.format(len(nameValuePairs), nameValuePairs), debug=True) + return nameValuePairs + +def processNameValuePairs(nameValuePairsAr): + # parse the name value pairs - return of dictionary of findings + findingsDict = {} + for nameValueStr in nameValuePairsAr: + if '=' in nameValueStr: + name,value = nameValueStr.split('=', 1) + print_line(' [{}]=[{}]'.format(name, value), debug=True) + findingsDict[name] = value + else: + print_line('processNameValuePairs nameValueStr({})=({}) ! missing "=" !'.format(len(nameValueStr), nameValueStr), warning=True) + return findingsDict + +def sendValidationError(serPort, cmdPrefixStr, errorMessage): + # format and send an error message via outgoing serial + successStatus = False + responseStr = '{}:status={}{}msg={}\n'.format(cmdPrefixStr, successStatus, parm_sep, errorMessage) + newOutLine = responseStr.encode('utf-8') + print_line('sendValidationError line({})=[{}]'.format(len(newOutLine), newOutLine), error=True) + serPort.write(newOutLine) + +def sendValidationSuccess(serPort, cmdPrefixStr, returnKeyStr, returnValueStr): + # format and send an error message via outgoing serial + successStatus = True + if(len(returnKeyStr) > 0): + # if we have a key we're sending along an extra KV pair + responseStr = '{}:status={}{}{}={}\n'.format(cmdPrefixStr, successStatus, parm_sep, returnKeyStr, returnValueStr) + else: + # no key so just send final status + responseStr = '{}:status={}\n'.format(cmdPrefixStr, successStatus) + newOutLine = responseStr.encode('utf-8') + print_line('sendValidationSuccess line({})=({})'.format(len(newOutLine), newOutLine), verbose=True) + serPort.write(newOutLine) + + +# ----------------------------------------------------------------------------- +# Main loop +# ----------------------------------------------------------------------------- + +# and allocate our single runtime config store +runtimeConfig = RuntimeConfig() + + +# alocate our access to our Host Info +rpiHost = RPiHostInfo() + +rpi_model, rpi_model_raw = rpiHost.getDeviceModel() +rpi_linux_release = rpiHost.getLinuxRelease() +rpi_linux_version = rpiHost.getLinuxVersion() +rpi_hostname, rpi_fqdn = rpiHost.getHostnames() + +# record RPi into to runtimeConfig +runtimeConfig.setConfigNamedVarValue(runtimeConfig.keyRPiModel, rpi_model) +runtimeConfig.setConfigNamedVarValue(runtimeConfig.keyRPiMdlFull, rpi_model_raw) +runtimeConfig.setConfigNamedVarValue(runtimeConfig.keyRPiRel, rpi_linux_release) +runtimeConfig.setConfigNamedVarValue(runtimeConfig.keyRPiVer, rpi_linux_version) +runtimeConfig.setConfigNamedVarValue(runtimeConfig.keyRPiName, rpi_hostname) +runtimeConfig.setConfigNamedVarValue(runtimeConfig.keyRPiFqdn, rpi_fqdn) +colorama_init() # Initialize our color console system + +# start our serial receive listener + +# 1,440,000 = 150x 9600 baud FAILS P2 Tx +# 864,000 = 90x 9600 baud FAILS P2 Tx +# 720,000 = 75x 9600 baud FAILS P2 Rx +# 672,000 = 70x 9600 baud FAILS P2 Rx +# 624,000 = 65x 9600 baud GOOD (Serial test proven) +# 499,200 = 52x 9600 baud +# 480,000 = 50x 9600 baud +# +baudRate = 624000 +print_line('Baud rate: {:,} bits/sec'.format(baudRate), verbose=True) + +serialPort = serial.Serial ("/dev/serial0", baudRate, timeout=1) #Open port with baud rate & timeout + +queueRxLines = RxLineQueue() + +_thread.start_new_thread(taskSerialListener, ( serialPort, )) + +sleep(1) # allow threads to start... + +# create our motor interface handing it our serial port to use +wheels = BLDCMotorControl(serialPort) + +# a quick method using wheels lack-of-motion to determine when +# can ask motors to do another command... +def waitForMotorsStopped(): + bothStopped = False + priorLtCt = -1 + priorRtCt = -1 + while bothStopped == False: + # get status + # PUB getRotationCount(rotationUnits) : leftRotationCount, rightRotationCount + ltCount, rtCount = wheels.getRotationCount(DrvRotUnits.DRU_HALL_TICKS) + print_line('- counts {}, {}'.format(ltCount, rtCount), debug=True) + #while not stopped loop + # get "stat" response (lt and rt status) + # if both are stopped the set bothStopped = true + if priorLtCt == ltCount and priorRtCt == rtCount: + bothStopped = TRUE + break + priorLtCt = ltCount + priorRtCt = rtCount + sleep(0.20) # wait for 1/5 second before asking again... + +# run our drive pattern +try: + # wait for runtimeConfig to get our hardware ID from P2 (saying it's alive) + print_line('- waiting P2 startup', verbose=True) + while not runtimeConfig.containsKey("hwName"): + sleep(0.5) # wait for 1/2 second before asking again... + + print_line('- P2 is alive!', verbose=True) + + # ------------------------- + # configure drive system + # ------------------------- + # override defaults, use 100 % + wheels.setMaxSpeed(100) + wheels.setMaxSpeedForDistance(100) + # and don't draw current at stop + wheels.holdAtStop(False) + + # ------------------------- + # drive a square pattern + # 3-second sides 75% power, 90° corners + # ------------------------- + # forward for 3 seconds at 75% power + desiredPower = 75 + lengthOfSideInSeconds = 3 + dirStraightAhead = 0 + wheels.driveDirection(desiredPower, dirStraightAhead) + wheels.stopAfterTime(lengthOfSideInSeconds, DrvTimeUnits.DTU_SECS) + waitForMotorsStopped() + + # hard 90° right turn + # [circ = 2 * PI * r] + # rotate about 1 wheel means effective-platform-radius is actual-platform-diameter! + # dist to travel = (effective-platform-circumference / 4) / wheel circumference + # effective-platform-radius = 17.5" + # wheel diameter = 6.5" + # travel dist is 1.346 rotations + # rotations * 90 = 121 hall ticks + dirHardRightTurn = 100 + ticksIn90DegreeTurn = 121 + wheels.driveDirection(desiredPower, dirHardRightTurn) + wheels.stopAfterRotation(ticksIn90DegreeTurn, DrvRotUnits.DRU_HALL_TICKS) + waitForMotorsStopped() + + # forward for 2 seconds at 50% power + wheels.driveDirection(desiredPower, dirStraightAhead) + wheels.stopAfterTime(lengthOfSideInSeconds, DrvTimeUnits.DTU_SECS) + waitForMotorsStopped() + + # hard 90° right turn + wheels.driveDirection(desiredPower, dirHardRightTurn) + wheels.stopAfterRotation(ticksIn90DegreeTurn, DrvRotUnits.DRU_HALL_TICKS) + waitForMotorsStopped() + + # forward for 2 seconds at 50% power + wheels.driveDirection(desiredPower, dirStraightAhead) + wheels.stopAfterTime(lengthOfSideInSeconds, DrvTimeUnits.DTU_SECS) + waitForMotorsStopped() + + # hard 90° right turn + wheels.driveDirection(desiredPower, dirHardRightTurn) + wheels.stopAfterRotation(ticksIn90DegreeTurn, DrvRotUnits.DRU_HALL_TICKS) + waitForMotorsStopped() + + # forward for 2 seconds at 50% power + wheels.driveDirection(desiredPower, dirStraightAhead) + wheels.stopAfterTime(lengthOfSideInSeconds, DrvTimeUnits.DTU_SECS) + waitForMotorsStopped() + + # hard 90° right turn + wheels.driveDirection(desiredPower, dirHardRightTurn) + wheels.stopAfterRotation(ticksIn90DegreeTurn, DrvRotUnits.DRU_HALL_TICKS) + waitForMotorsStopped() + # ------------------------- + +finally: + # normal shutdown + print_line('Done', info=True) diff --git a/pythonSrc/requirements.txt b/pythonSrc/requirements.txt new file mode 100644 index 0000000..694bfe8 --- /dev/null +++ b/pythonSrc/requirements.txt @@ -0,0 +1,8 @@ +# +paho-mqtt>=1.4.0 +wheel>=0.29.0 +sdnotify>=0.3.1 +Unidecode>=0.4.21 +colorama>=0.4.3 +tzlocal>=2.1.0 +sendgrid>=6.9.3