├── README.md ├── Swarmathon_Arduino └── Swarmathon_Arduino.ino ├── libraries ├── IMU │ ├── L3G.cpp │ ├── L3G.h │ ├── LPS.cpp │ ├── LPS.h │ ├── LSM303.cpp │ └── LSM303.h ├── Movement │ ├── Movement.cpp │ └── Movement.h ├── NewPing │ ├── NewPing.cpp │ └── NewPing.h ├── Odometry │ ├── Odometry.cpp │ └── Odometry.h └── Servo │ ├── Servo.cpp │ ├── Servo.h │ ├── ServoTimers.h │ └── library.properties └── readmeImages ├── ArduinoDebugDataInput.png ├── ArduinoDebugDataOutput.png ├── ArduinoDebugFingersOpen.png ├── ArduinoDebugHighCenter.png ├── ArduinoDebugWristDown.png ├── ArduinoIDEBoardType.png ├── ArduinoIDELineEndingBaudRate.png ├── ArduinoIDEOpenSketch.png ├── ArduinoIDEOpenSketch2.png ├── ArduinoIDESerialMonitor.png ├── ArduinoIDESerialPort.png ├── ArduinoIDESketchbookLocation.png ├── ArduinoIDEUploadSketch.png └── ArduinoIDEUploadSuccess.png /README.md: -------------------------------------------------------------------------------- 1 | Swarmathon-Arduino 2 | ============== 3 | 4 | This repository is an Arduino microcontroller library for the Swarmie robots used in the [NASA Swarmathon](http://www.nasaswarmathon.com), a national swarm robotics competition created by the [University of New Mexico](http://www.unm.edu/). This particular library is an interface to the [Swarmathon-ROS](https://github.com/BCLab-UNM/Swarmathon-ROS) controller framework that facilitates the opertion of lower-level functionality onboard the physical robot, including brushed DC motors, integrated quadrature encoders, a 10-axis IMU (inertial measurement unit), and ultrasonic distance sensors. 5 | 6 | For information on setting up the Arduino IDE and programming Arduino microcontrollers, please consult [Getting Started with Arduino](https://www.arduino.cc/en/Guide/HomePage). Aside from the IDE, no other plugins or tools are required to begin using this libra*ry. 7 | 8 | **Please use version 1.8.5 of the IDE.** 9 | 10 | If you are using Linux, you'll need to run several additional commands to ensure that the Arduino IDE has the correct permissions to run. First, open a **Terminal** window and run the command `sudo usermod -a -G dialout username` to add your user account to the `dialout` user group, where `username` should be replaced by your own user name. Then, run the command `echo 'ATTRS{idVendor}=="1ffb", ENV{ID_MM_DEVICE_IGNORE}="1"' | sudo tee /etc/udev/rules.d/77-arduino.rules` to force Ubuntu's modem-manager to ignore the Arduino when it is plugged in. Finally, reload the `udev` device manager with the command `sudo udevadm trigger`. 11 | 12 | After installing the Arduino IDE, run the application and open the Arduino IDE Preferences window (under "File > Preferences" in Linux and Windows, or "Arduino > Preferences" in Mac OS X). Under the Settings tab, in the text box titled "Sketchbook location", enter the full path to your Swarmathon-Arduino directory, then click "OK": 13 | 14 | ![Arduino IDE Sketchbook location](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoIDESketchbookLocation.png) 15 | 16 | **Note** that you must exit and reopen the Arduino IDE before the change to the Sketchbook location is applied. 17 | 18 | ## Setup 19 | 20 | 1. Clone this GitHub repository to your home directory (~): 21 | 22 | ``` 23 | cd ~ 24 | git clone https://github.com/BCLab-UNM/Swarmathon-Arduino.git 25 | ``` 26 | 27 | 2. To set up the Arduino IDE to communicate with the Swarmie's [Pololu A-Star microcontroller](https://www.pololu.com/product/3104), which runs an Arduino-compatible bootloader, first set the board type under "Select > Board" to "Arduino Leonardo". 28 | 29 | ![Arduino IDE Board Type](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoIDEBoardType.png) 30 | 31 | 3. Ensure that the A-Star is plugged into your PC (not the Swarmie's NUC), then select the proper serial port under "Select > Port". Your port number will most likely differ from the one shown in the screenshot below, but you should still see "Arduino Leonardo" next to the correct port. 32 | 33 | ![Arduino IDE Serial Port](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoIDESerialPort.png) 34 | 35 | 4. If you haven't loaded it already, open the Swarmathon_Arduino.ino sketch under "File > Open" by navigating to your Swarmathon-Arduino directory. 36 | 37 | ![Arduino IDE Open Sketch](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoIDEOpenSketch.png) 38 | ![Arduino IDE Open Sketch2](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoIDEOpenSketch2.png) 39 | 40 | 5. Upload the sketch to the A-Star by clicking on the "Upload" button, a right arrow in the upper-left corner of the Arduino IDE. 41 | 42 | ![Arduino IDE Upload Sketch](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoIDEUploadSketch.png) 43 | 44 | 6. If your upload is successful, you should see output in the black terminal box at the bottom of the Arduino IDE regarding the size of the sketch, as well as a "Done uploading" message above this black box. If the Arduino IDE outputs an error, please double-check that you have followed steps 1 through 4 above correctly. 45 | 46 | ![Arduino IDE Upload Success](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoIDEUploadSuccess.png) 47 | 48 | 49 | ## Debugging 50 | 51 | If you are encountering issues with your Swarmie, such as missing IMU, encoder, and/or ultrasound data, or problems with driving the robot, please consult the suggestions below after connecting the A-Star microcontroller to your PC and following steps 1 through 6 above. 52 | 53 | 1. Open the Arduino IDE and click the Serial Monitor button, a magnifying glass in the upper-right corner of the Arduino IDE. 54 | 55 | ![Arduino IDE Serial Monitor](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoIDESerialMonitor.png) 56 | 57 | In the drop-down menus at the bottom of the Serial Monitor window that appears, ensure that the line ending option is set to "Newline", and that the baud rate is set to "115200 baud". 58 | 59 | ![Arduino IDE Line Ending and Baudrate](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoIDELineEndingBaudRate.png) 60 | 61 | 2. Type `d` into the entry bar at the top of the Serial Monitor window and click the **Send** button. 62 | 63 | ![Arduino Debug Data Input](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoDebugDataInput.png) 64 | 65 | You should receive a comma-delimited string of 18 floating-point values, similar to, but not identical to, the string shown here. 66 | 67 | ![Arduino Debug Data Output](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoDebugDataOutput.png) 68 | 69 | 3. Type `f,1` into the entry bar and click **Send**. This command should open the gripper fingers to the angle shown. 70 | 71 | ![Arduino Debug Fingers Open](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoDebugFingersOpen.png) 72 | 73 | 4. Type `w,1` into the entry bar and click **Send**. This command should lower the gripper wrist to the angle shown. 74 | 75 | ![Arduino Debug Wrist Down](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoDebugWristDown.png) 76 | 77 | 5. High-center the robot on a box so that none of the wheels are touching the ground. Ensure that the motors are turned on by flipping the red switch on the back of the robot up. 78 | 79 | ![Arduino Debug High Center](https://github.com/BCLab-UNM/Swarmathon-Arduino/blob/master/readmeImages/ArduinoDebugHighCenter.png) 80 | 81 | Type `v,80,80` into the entry bar and click **Send**. The wheels should spin forward (rotating toward the front of the robot) for one second, then stop automatically. 82 | 83 | Type `v,-80,-80` into the entry bar and click **Send**. The wheels should spin backward (rotating toward the back of the robot) for one second, then stop automatically. 84 | 85 | Type `v,-80,80` into the entry bar and click **Send**. The wheels on the left side of the robot should spin backward, while the wheels on the right side of the robot should spin forward, for one second, then stop automatically. 86 | 87 | Finally, type `v,80,-80` into the entry bar and click **Send**. The wheels on the left side of the robot should spin forward, while the wheels on the right side of the robot should spin backward, for one second, then stop automatically. 88 | -------------------------------------------------------------------------------- /Swarmathon_Arduino/Swarmathon_Arduino.ino: -------------------------------------------------------------------------------- 1 | ////////////////////////// 2 | ////Required Libraries//// 3 | ////////////////////////// 4 | 5 | //Built-in Arduino libraries 6 | #include 7 | 8 | //Custom libraries located in Swarmathon-Arduino repo 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // Constants 18 | #define PI 3.14159265358979323846 19 | #define RAD2DEG(radianAngle) (radianAngle * 180.0 / PI) 20 | #define DEG2RAD(degreeAngle) (degreeAngle * PI / 180.0) 21 | 22 | //////////////// 23 | ////Settings//// 24 | //////////////// 25 | 26 | //Gripper (HS-485HB Servo) 27 | byte fingersPin = 9; 28 | byte wristPin = 12; 29 | int fingerMin = 800; //if you want to shift 0 to a new location raise min; this is closed 30 | int fingerMax = 2600; //if you want to limit max travel lower max; this is open 31 | int wristMin = 1400; //this is up 32 | int wristMax = 2600; //this is down 33 | 34 | //Movement (VNH5019 Motor Driver Carrier) 35 | byte rightDirectionA = A3; //"clockwise" input 36 | byte rightDirectionB = A2; //"counterclockwise" input 37 | byte rightSpeedPin = 11; //PWM input 38 | byte leftDirectionA = A5; //"clockwise" input 39 | byte leftDirectionB = A4; //"counterclockwise" input 40 | byte leftSpeedPin = 10; //PWM input 41 | 42 | // Modify before merge 43 | 44 | //Odometry (8400 CPR Encoder) 45 | byte rightEncoderA = 7; 46 | byte rightEncoderB = 8; 47 | byte leftEncoderA = 0; 48 | byte leftEncoderB = 1; 49 | float wheelBase = 27.8; //distance between left and right wheels (in cm) 50 | float wheelDiameter = 12.2; //diameter of wheel (in cm) 51 | int cpr = 8400; //"cycles per revolution" -- number of encoder increments per one wheel revolution 52 | 53 | //Serial (USB <--> Intel NUC) 54 | String rxBuffer; 55 | unsigned long watchdogTimer = 1000; //fail-safe in case of communication link failure (in ms) 56 | unsigned long lastCommTime = 0; //time of last communication from NUC (in ms) 57 | 58 | //Ultrasound (Ping)))) 59 | byte leftSignal = 4; 60 | byte centerSignal = 5; 61 | byte rightSignal = 6; 62 | 63 | 64 | //////////////////////////// 65 | ////Class Instantiations//// 66 | //////////////////////////// 67 | 68 | L3G gyroscope; 69 | LSM303 magnetometer_accelerometer; 70 | LPS pressure; 71 | Movement move = Movement(rightSpeedPin, rightDirectionA, rightDirectionB, leftSpeedPin, leftDirectionA, leftDirectionB); 72 | Odometry odom = Odometry(rightEncoderA, rightEncoderB, leftEncoderA, leftEncoderB, wheelBase, wheelDiameter, cpr); 73 | Servo fingers; 74 | Servo wrist; 75 | NewPing leftUS(leftSignal, leftSignal, 330); 76 | NewPing centerUS(centerSignal, centerSignal, 330); 77 | NewPing rightUS(rightSignal, rightSignal, 330); 78 | 79 | 80 | ///////////// 81 | ////Setup//// 82 | ///////////// 83 | 84 | void setup() 85 | { 86 | Serial.begin(115200); 87 | while (!Serial) {} //wait for Serial to complete initialization before moving on 88 | 89 | Wire.begin(); 90 | 91 | if (imuStatus()) { 92 | imuInit(); 93 | } 94 | 95 | fingers.attach(fingersPin,fingerMin,fingerMax); 96 | fingers.writeMicroseconds(fingerMin); 97 | wrist.attach(wristPin,wristMin,wristMax); 98 | wrist.writeMicroseconds(wristMin); 99 | 100 | rxBuffer = ""; 101 | } 102 | 103 | 104 | ///////////////// 105 | ////Main Loop//// 106 | ///////////////// 107 | 108 | void loop() { 109 | if (Serial.available()) { 110 | char c = Serial.read(); 111 | if (c == ',' || c == '\n') { 112 | parse(); 113 | rxBuffer = ""; 114 | lastCommTime = millis(); 115 | } 116 | else if (c > 0) { 117 | rxBuffer += c; 118 | } 119 | } 120 | if (millis() - lastCommTime > watchdogTimer) { 121 | move.stop(); 122 | } 123 | } 124 | 125 | 126 | //////////////////////// 127 | //Parse receive buffer// 128 | //////////////////////// 129 | 130 | void parse() { 131 | if (rxBuffer == "v") { 132 | int speedL = Serial.parseInt(); 133 | int speedR = Serial.parseInt(); 134 | 135 | if (speedL >= 0 && speedR >= 0) { 136 | move.forward(speedL, speedR); 137 | } 138 | else if (speedL <= 0 && speedR <= 0) { 139 | move.backward(speedL*-1, speedR*-1); 140 | } 141 | else if (speedL <= 0 && speedR >= 0) { 142 | move.rotateLeft(speedL*-1, speedR); 143 | } 144 | else { 145 | move.rotateRight(speedL, speedR*-1); 146 | } 147 | } 148 | else if (rxBuffer == "s") { 149 | move.stop(); 150 | } 151 | else if (rxBuffer == "d") { 152 | Serial.print("GRF,"); 153 | Serial.print(String(fingers.attached()) + ","); 154 | if (fingers.attached()) { 155 | Serial.println(String(DEG2RAD(fingers.read()))); 156 | } 157 | else { 158 | Serial.println(); 159 | } 160 | 161 | Serial.print("GRW,"); 162 | Serial.print(String(wrist.attached()) + ","); 163 | if (wrist.attached()) { 164 | Serial.println(String(DEG2RAD(wrist.read()))); 165 | } 166 | else { 167 | Serial.println(); 168 | } 169 | 170 | Serial.print("IMU,"); 171 | bool imuStatusFlag = imuStatus(); 172 | Serial.print(String(imuStatusFlag) + ","); 173 | if (imuStatusFlag) { 174 | imuInit(); 175 | Serial.println(updateIMU()); 176 | } 177 | else { 178 | Serial.println(",,,,,,,,"); 179 | } 180 | 181 | Serial.println("ODOM," + String(1) + "," + updateOdom()); 182 | 183 | Serial.print("USL,"); 184 | int leftUSValue = leftUS.ping_cm(); 185 | Serial.print(String(leftUSValue > 0 ? 1 : 0) + ","); 186 | if (leftUSValue > 0) { 187 | Serial.println(String(leftUSValue)); 188 | } 189 | else { 190 | Serial.println(); 191 | } 192 | 193 | Serial.print("USC,"); 194 | int centerUSValue = centerUS.ping_cm(); 195 | Serial.print(String(centerUSValue > 0 ? 1 : 0) + ","); 196 | if (centerUSValue > 0) { 197 | Serial.println(String(centerUSValue)); 198 | } 199 | else { 200 | Serial.println(); 201 | } 202 | 203 | Serial.print("USR,"); 204 | int rightUSValue = rightUS.ping_cm(); 205 | Serial.print(String(rightUSValue > 0 ? 1 : 0) + ","); 206 | if (rightUSValue > 0) { 207 | Serial.println(String(rightUSValue)); 208 | } 209 | else { 210 | Serial.println(); 211 | } 212 | } 213 | else if (rxBuffer == "f") { 214 | float radianAngle = Serial.parseFloat(); 215 | int angle = RAD2DEG(radianAngle); // Convert float radians to int degrees 216 | angle = fingerMin + (fingerMax/370) * angle; 217 | fingers.writeMicroseconds(angle); 218 | } 219 | else if (rxBuffer == "w") { 220 | float radianAngle = Serial.parseFloat(); 221 | int angle = RAD2DEG(radianAngle); // Convert float radians to int degrees 222 | angle = wristMin + (wristMax/370) * angle; 223 | wrist.writeMicroseconds(angle); 224 | } 225 | } 226 | 227 | 228 | ////////////////////////// 229 | //Update transmit buffer// 230 | ////////////////////////// 231 | 232 | String updateIMU() { 233 | //Update current sensor values 234 | gyroscope.read(); 235 | magnetometer_accelerometer.read(); 236 | 237 | if (!gyroscope.timeoutOccurred() && !magnetometer_accelerometer.timeoutOccurred()) { 238 | //Collect updated values 239 | LSM303::vector acc = magnetometer_accelerometer.a; 240 | L3G::vector gyro = gyroscope.g; 241 | LSM303::vector mag = magnetometer_accelerometer.m; 242 | 243 | //Convert accelerometer digits to milligravities, then to gravities, and finally to meters per second squared 244 | LSM303::vector linear_acceleration = {acc.y*0.061/1000*9.81, -acc.x*0.061/1000*9.81, acc.z*0.061/1000*9.81}; 245 | 246 | //Convert gyroscope digits to millidegrees per second, then to degrees per second, and finally to radians per second 247 | L3G::vector angular_velocity = {gyro.y*8.75/1000*(PI/180), -gyro.x*8.75/1000*(PI/180), gyro.z*8.75/1000*(PI/180)}; 248 | 249 | //Combine normalized magnetometer and accelerometer digits to produce Euler angles, i.e. pitch, roll, and yaw 250 | LSM303::vector orientation = {(float)mag.x, (float)mag.y, (float)mag.z}; 251 | orientation.x -= (magnetometer_accelerometer.m_min.x + magnetometer_accelerometer.m_max.x) / 2; 252 | orientation.y -= (magnetometer_accelerometer.m_min.y + magnetometer_accelerometer.m_max.y) / 2; 253 | orientation.z -= (magnetometer_accelerometer.m_min.z + magnetometer_accelerometer.m_max.z) / 2; 254 | LSM303::vector_normalize(&orientation); 255 | float roll = atan2(linear_acceleration.y, sqrt(pow(linear_acceleration.x,2) + pow(linear_acceleration.z,2))); 256 | float pitch = -atan2(linear_acceleration.x, sqrt(pow(linear_acceleration.y,2) + pow(linear_acceleration.z,2))); 257 | float yaw = atan2(-orientation.y*cos(roll) + orientation.z*sin(roll), orientation.x*cos(pitch) + orientation.y*sin(pitch)*sin(roll) + orientation.z*sin(pitch)*cos(roll)) + PI; 258 | orientation = {roll, pitch, yaw}; 259 | 260 | //Append data to buffer 261 | String txBuffer = String(linear_acceleration.x) + "," + 262 | String(linear_acceleration.y) + "," + 263 | String(linear_acceleration.z) + "," + 264 | String(angular_velocity.x) + "," + 265 | String(angular_velocity.y) + "," + 266 | String(angular_velocity.z) + "," + 267 | String(orientation.x) + "," + 268 | String(orientation.y) + "," + 269 | String(orientation.z); 270 | 271 | return txBuffer; 272 | } 273 | 274 | return ""; 275 | } 276 | 277 | String updateOdom() { 278 | String txBuffer; 279 | odom.update(); 280 | 281 | txBuffer = String(odom.x) + "," + 282 | String(odom.y) + "," + 283 | String(odom.theta) + "," + 284 | String(odom.vx) + "," + 285 | String(odom.vy) + "," + 286 | String(odom.vtheta); 287 | 288 | return txBuffer; 289 | } 290 | 291 | 292 | //////////////////////////// 293 | ////Initializer Functions/// 294 | //////////////////////////// 295 | 296 | //Initialize gyroscope, accelerometer, magnetometer, and pressure gauge 297 | void imuInit() { 298 | gyroscope.init(); 299 | gyroscope.enableDefault(); 300 | gyroscope.setTimeout(1); 301 | 302 | magnetometer_accelerometer.init(); 303 | magnetometer_accelerometer.enableDefault(); 304 | magnetometer_accelerometer.m_min = (LSM303::vector){ -2247, -2068, -1114}; 305 | magnetometer_accelerometer.m_max = (LSM303::vector){+3369, +2877, +3634}; 306 | magnetometer_accelerometer.setTimeout(1); 307 | 308 | pressure.init(); 309 | pressure.enableDefault(); 310 | } 311 | 312 | 313 | //////////////////////////// 314 | ////Diagnostic Functions//// 315 | //////////////////////////// 316 | 317 | //Check for valid I2C connection to verify IMU 318 | bool imuStatus() { 319 | byte numberOfDevices = 0; 320 | 321 | for(byte address = 1; address < 127; address++ ) { 322 | Wire.beginTransmission(address); 323 | byte error = Wire.endTransmission(); 324 | 325 | if (!error) { 326 | numberOfDevices++; 327 | } 328 | } 329 | 330 | if (numberOfDevices > 0) { 331 | return true; 332 | } 333 | else { 334 | return false; 335 | } 336 | } 337 | -------------------------------------------------------------------------------- /libraries/IMU/L3G.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // Defines //////////////////////////////////////////////////////////////// 6 | 7 | // The Arduino two-wire interface uses a 7-bit number for the address, 8 | // and sets the last bit correctly based on reads and writes 9 | #define D20_SA0_HIGH_ADDRESS 0b1101011 // also applies to D20H 10 | #define D20_SA0_LOW_ADDRESS 0b1101010 // also applies to D20H 11 | #define L3G4200D_SA0_HIGH_ADDRESS 0b1101001 12 | #define L3G4200D_SA0_LOW_ADDRESS 0b1101000 13 | 14 | #define TEST_REG_ERROR -1 15 | 16 | #define D20H_WHO_ID 0xD7 17 | #define D20_WHO_ID 0xD4 18 | #define L3G4200D_WHO_ID 0xD3 19 | 20 | // Constructors //////////////////////////////////////////////////////////////// 21 | 22 | L3G::L3G(void) 23 | { 24 | _device = device_auto; 25 | 26 | io_timeout = 0; // 0 = no timeout 27 | did_timeout = false; 28 | } 29 | 30 | // Public Methods ////////////////////////////////////////////////////////////// 31 | 32 | // Did a timeout occur in read() since the last call to timeoutOccurred()? 33 | bool L3G::timeoutOccurred() 34 | { 35 | bool tmp = did_timeout; 36 | did_timeout = false; 37 | return tmp; 38 | } 39 | 40 | void L3G::setTimeout(unsigned int timeout) 41 | { 42 | io_timeout = timeout; 43 | } 44 | 45 | unsigned int L3G::getTimeout() 46 | { 47 | return io_timeout; 48 | } 49 | 50 | bool L3G::init(deviceType device, sa0State sa0) 51 | { 52 | int id; 53 | 54 | // perform auto-detection unless device type and SA0 state were both specified 55 | if (device == device_auto || sa0 == sa0_auto) 56 | { 57 | // check for L3GD20H, D20 if device is unidentified or was specified to be one of these types 58 | if (device == device_auto || device == device_D20H || device == device_D20) 59 | { 60 | // check SA0 high address unless SA0 was specified to be low 61 | if (sa0 != sa0_low && (id = testReg(D20_SA0_HIGH_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR) 62 | { 63 | // device responds to address 1101011; it's a D20H or D20 with SA0 high 64 | sa0 = sa0_high; 65 | if (device == device_auto) 66 | { 67 | // use ID from WHO_AM_I register to determine device type 68 | device = (id == D20H_WHO_ID) ? device_D20H : device_D20; 69 | } 70 | } 71 | // check SA0 low address unless SA0 was specified to be high 72 | else if (sa0 != sa0_high && (id = testReg(D20_SA0_LOW_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR) 73 | { 74 | // device responds to address 1101010; it's a D20H or D20 with SA0 low 75 | sa0 = sa0_low; 76 | if (device == device_auto) 77 | { 78 | // use ID from WHO_AM_I register to determine device type 79 | device = (id == D20H_WHO_ID) ? device_D20H : device_D20; 80 | } 81 | } 82 | } 83 | 84 | // check for L3G4200D if device is still unidentified or was specified to be this type 85 | if (device == device_auto || device == device_4200D) 86 | { 87 | if (sa0 != sa0_low && testReg(L3G4200D_SA0_HIGH_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID) 88 | { 89 | // device responds to address 1101001; it's a 4200D with SA0 high 90 | device = device_4200D; 91 | sa0 = sa0_high; 92 | } 93 | else if (sa0 != sa0_high && testReg(L3G4200D_SA0_LOW_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID) 94 | { 95 | // device responds to address 1101000; it's a 4200D with SA0 low 96 | device = device_4200D; 97 | sa0 = sa0_low; 98 | } 99 | } 100 | 101 | // make sure device and SA0 were successfully detected; otherwise, indicate failure 102 | if (device == device_auto || sa0 == sa0_auto) 103 | { 104 | return false; 105 | } 106 | } 107 | 108 | _device = device; 109 | 110 | // set device address 111 | switch (device) 112 | { 113 | case device_D20H: 114 | case device_D20: 115 | address = (sa0 == sa0_high) ? D20_SA0_HIGH_ADDRESS : D20_SA0_LOW_ADDRESS; 116 | break; 117 | 118 | case device_4200D: 119 | address = (sa0 == sa0_high) ? L3G4200D_SA0_HIGH_ADDRESS : L3G4200D_SA0_LOW_ADDRESS; 120 | break; 121 | } 122 | 123 | return true; 124 | } 125 | 126 | /* 127 | Enables the L3G's gyro. Also: 128 | - Sets gyro full scale (gain) to default power-on value of +/- 250 dps 129 | (specified as +/- 245 dps for L3GD20H). 130 | - Selects 200 Hz ODR (output data rate). (Exact rate is specified as 189.4 Hz 131 | for L3GD20H and 190 Hz for L3GD20.) 132 | Note that this function will also reset other settings controlled by 133 | the registers it writes to. 134 | */ 135 | void L3G::enableDefault(void) 136 | { 137 | if (_device == device_D20H) 138 | { 139 | // 0x00 = 0b00000000 140 | // Low_ODR = 0 (low speed ODR disabled) 141 | writeReg(LOW_ODR, 0x00); 142 | } 143 | 144 | // 0x00 = 0b00000000 145 | // FS = 00 (+/- 250 dps full scale) 146 | writeReg(CTRL_REG4, 0x00); 147 | 148 | // 0x6F = 0b01101111 149 | // DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled) 150 | writeReg(CTRL_REG1, 0x6F); 151 | } 152 | 153 | // Writes a gyro register 154 | void L3G::writeReg(byte reg, byte value) 155 | { 156 | Wire.beginTransmission(address); 157 | Wire.write(reg); 158 | Wire.write(value); 159 | last_status = Wire.endTransmission(); 160 | } 161 | 162 | // Reads a gyro register 163 | byte L3G::readReg(byte reg) 164 | { 165 | byte value; 166 | 167 | Wire.beginTransmission(address); 168 | Wire.write(reg); 169 | last_status = Wire.endTransmission(); 170 | Wire.requestFrom(address, (byte)1); 171 | value = Wire.read(); 172 | Wire.endTransmission(); 173 | 174 | return value; 175 | } 176 | 177 | // Reads the 3 gyro channels and stores them in vector g 178 | void L3G::read() 179 | { 180 | Wire.beginTransmission(address); 181 | // assert the MSB of the address to get the gyro 182 | // to do slave-transmit subaddress updating. 183 | Wire.write(OUT_X_L | (1 << 7)); 184 | Wire.endTransmission(); 185 | Wire.requestFrom(address, (byte)6); 186 | 187 | unsigned int millis_start = millis(); 188 | while (Wire.available() < 6) 189 | { 190 | if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout) 191 | { 192 | did_timeout = true; 193 | return; 194 | } 195 | } 196 | 197 | uint8_t xlg = Wire.read(); 198 | uint8_t xhg = Wire.read(); 199 | uint8_t ylg = Wire.read(); 200 | uint8_t yhg = Wire.read(); 201 | uint8_t zlg = Wire.read(); 202 | uint8_t zhg = Wire.read(); 203 | 204 | // combine high and low bytes 205 | g.x = (int16_t)(xhg << 8 | xlg); 206 | g.y = (int16_t)(yhg << 8 | ylg); 207 | g.z = (int16_t)(zhg << 8 | zlg); 208 | } 209 | 210 | void L3G::vector_normalize(vector *a) 211 | { 212 | float mag = sqrt(vector_dot(a,a)); 213 | a->x /= mag; 214 | a->y /= mag; 215 | a->z /= mag; 216 | } 217 | 218 | // Private Methods ////////////////////////////////////////////////////////////// 219 | 220 | int L3G::testReg(byte address, regAddr reg) 221 | { 222 | Wire.beginTransmission(address); 223 | Wire.write((byte)reg); 224 | if (Wire.endTransmission() != 0) 225 | { 226 | return TEST_REG_ERROR; 227 | } 228 | 229 | Wire.requestFrom(address, (byte)1); 230 | if (Wire.available()) 231 | { 232 | return Wire.read(); 233 | } 234 | else 235 | { 236 | return TEST_REG_ERROR; 237 | } 238 | } 239 | -------------------------------------------------------------------------------- /libraries/IMU/L3G.h: -------------------------------------------------------------------------------- 1 | #ifndef L3G_h 2 | #define L3G_h 3 | 4 | #include // for byte data type 5 | 6 | class L3G 7 | { 8 | public: 9 | template struct vector 10 | { 11 | T x, y, z; 12 | }; 13 | 14 | enum deviceType { device_4200D, device_D20, device_D20H, device_auto }; 15 | enum sa0State { sa0_low, sa0_high, sa0_auto }; 16 | 17 | // register addresses 18 | enum regAddr 19 | { 20 | WHO_AM_I = 0x0F, 21 | 22 | CTRL1 = 0x20, // D20H 23 | CTRL_REG1 = 0x20, // D20, 4200D 24 | CTRL2 = 0x21, // D20H 25 | CTRL_REG2 = 0x21, // D20, 4200D 26 | CTRL3 = 0x22, // D20H 27 | CTRL_REG3 = 0x22, // D20, 4200D 28 | CTRL4 = 0x23, // D20H 29 | CTRL_REG4 = 0x23, // D20, 4200D 30 | CTRL5 = 0x24, // D20H 31 | CTRL_REG5 = 0x24, // D20, 4200D 32 | REFERENCE = 0x25, 33 | OUT_TEMP = 0x26, 34 | STATUS = 0x27, // D20H 35 | STATUS_REG = 0x27, // D20, 4200D 36 | 37 | OUT_X_L = 0x28, 38 | OUT_X_H = 0x29, 39 | OUT_Y_L = 0x2A, 40 | OUT_Y_H = 0x2B, 41 | OUT_Z_L = 0x2C, 42 | OUT_Z_H = 0x2D, 43 | 44 | FIFO_CTRL = 0x2E, // D20H 45 | FIFO_CTRL_REG = 0x2E, // D20, 4200D 46 | FIFO_SRC = 0x2F, // D20H 47 | FIFO_SRC_REG = 0x2F, // D20, 4200D 48 | 49 | IG_CFG = 0x30, // D20H 50 | INT1_CFG = 0x30, // D20, 4200D 51 | IG_SRC = 0x31, // D20H 52 | INT1_SRC = 0x31, // D20, 4200D 53 | IG_THS_XH = 0x32, // D20H 54 | INT1_THS_XH = 0x32, // D20, 4200D 55 | IG_THS_XL = 0x33, // D20H 56 | INT1_THS_XL = 0x33, // D20, 4200D 57 | IG_THS_YH = 0x34, // D20H 58 | INT1_THS_YH = 0x34, // D20, 4200D 59 | IG_THS_YL = 0x35, // D20H 60 | INT1_THS_YL = 0x35, // D20, 4200D 61 | IG_THS_ZH = 0x36, // D20H 62 | INT1_THS_ZH = 0x36, // D20, 4200D 63 | IG_THS_ZL = 0x37, // D20H 64 | INT1_THS_ZL = 0x37, // D20, 4200D 65 | IG_DURATION = 0x38, // D20H 66 | INT1_DURATION = 0x38, // D20, 4200D 67 | 68 | LOW_ODR = 0x39 // D20H 69 | }; 70 | 71 | vector g; // gyro angular velocity readings 72 | 73 | byte last_status; // status of last I2C transmission 74 | 75 | L3G(void); 76 | 77 | bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto); 78 | deviceType getDeviceType(void) { return _device; } 79 | 80 | void enableDefault(void); 81 | 82 | void writeReg(byte reg, byte value); 83 | byte readReg(byte reg); 84 | 85 | void read(void); 86 | 87 | void setTimeout(unsigned int timeout); 88 | unsigned int getTimeout(void); 89 | bool timeoutOccurred(void); 90 | 91 | // vector functions 92 | template static void vector_cross(const vector *a, const vector *b, vector *out); 93 | template static float vector_dot(const vector *a, const vector *b); 94 | static void vector_normalize(vector *a); 95 | 96 | private: 97 | deviceType _device; // chip type (D20H, D20, or 4200D) 98 | byte address; 99 | 100 | unsigned int io_timeout; 101 | bool did_timeout; 102 | 103 | int testReg(byte address, regAddr reg); 104 | }; 105 | 106 | template void L3G::vector_cross(const vector *a, const vector *b, vector *out) 107 | { 108 | out->x = (a->y * b->z) - (a->z * b->y); 109 | out->y = (a->z * b->x) - (a->x * b->z); 110 | out->z = (a->x * b->y) - (a->y * b->x); 111 | } 112 | 113 | template float L3G::vector_dot(const vector *a, const vector *b) 114 | { 115 | return (a->x * b->x) + (a->y * b->y) + (a->z * b->z); 116 | } 117 | 118 | #endif 119 | 120 | 121 | 122 | -------------------------------------------------------------------------------- /libraries/IMU/LPS.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // Defines /////////////////////////////////////////////////////////// 5 | 6 | // The Arduino two-wire interface uses a 7-bit number for the address, 7 | // and sets the last bit correctly based on reads and writes 8 | #define SA0_LOW_ADDRESS 0b1011100 9 | #define SA0_HIGH_ADDRESS 0b1011101 10 | 11 | #define TEST_REG_NACK -1 12 | 13 | #define LPS331AP_WHO_ID 0xBB 14 | #define LPS25H_WHO_ID 0xBD 15 | 16 | // Constructors ////////////////////////////////////////////////////// 17 | 18 | LPS::LPS(void) 19 | { 20 | _device = device_auto; 21 | 22 | // Pololu board pulls SA0 high, so default assumption is that it is 23 | // high 24 | address = SA0_HIGH_ADDRESS; 25 | } 26 | 27 | // Public Methods //////////////////////////////////////////////////// 28 | 29 | // sets or detects device type and slave address; returns bool indicating success 30 | bool LPS::init(deviceType device, byte sa0) 31 | { 32 | if (!detectDeviceAndAddress(device, (sa0State)sa0)) 33 | return false; 34 | 35 | switch (_device) 36 | { 37 | case device_25H: 38 | translated_regs[-INTERRUPT_CFG] = LPS25H_INTERRUPT_CFG; 39 | translated_regs[-INT_SOURCE] = LPS25H_INT_SOURCE; 40 | translated_regs[-THS_P_L] = LPS25H_THS_P_L; 41 | translated_regs[-THS_P_H] = LPS25H_THS_P_H; 42 | return true; 43 | break; 44 | 45 | case device_331AP: 46 | translated_regs[-INTERRUPT_CFG] = LPS331AP_INTERRUPT_CFG; 47 | translated_regs[-INT_SOURCE] = LPS331AP_INT_SOURCE; 48 | translated_regs[-THS_P_L] = LPS331AP_THS_P_L; 49 | translated_regs[-THS_P_H] = LPS331AP_THS_P_H; 50 | return true; 51 | break; 52 | } 53 | } 54 | 55 | // turns on sensor and enables continuous output 56 | void LPS::enableDefault(void) 57 | { 58 | if (_device == device_25H) 59 | { 60 | // 0xB0 = 0b10110000 61 | // PD = 1 (active mode); ODR = 011 (12.5 Hz pressure & temperature output data rate) 62 | writeReg(CTRL_REG1, 0xB0); 63 | } 64 | else if (_device == device_331AP) 65 | { 66 | // 0xE0 = 0b11100000 67 | // PD = 1 (active mode); ODR = 110 (12.5 Hz pressure & temperature output data rate) 68 | writeReg(CTRL_REG1, 0xE0); 69 | } 70 | } 71 | 72 | // writes register 73 | void LPS::writeReg(int reg, byte value) 74 | { 75 | // if dummy register address, look up actual translated address (based on device type) 76 | if (reg < 0) 77 | { 78 | reg = translated_regs[-reg]; 79 | } 80 | 81 | Wire.beginTransmission(address); 82 | Wire.write(reg); 83 | Wire.write(value); 84 | Wire.endTransmission(); 85 | } 86 | 87 | // reads register 88 | byte LPS::readReg(int reg) 89 | { 90 | byte value; 91 | 92 | // if dummy register address, look up actual translated address (based on device type) 93 | if (reg < 0) 94 | { 95 | reg = translated_regs[-reg]; 96 | } 97 | 98 | Wire.beginTransmission(address); 99 | Wire.write(reg); 100 | Wire.endTransmission(false); // restart 101 | Wire.requestFrom(address, (byte)1); 102 | value = Wire.read(); 103 | Wire.endTransmission(); 104 | 105 | return value; 106 | } 107 | 108 | // reads pressure in millibars (mbar)/hectopascals (hPa) 109 | float LPS::readPressureMillibars(void) 110 | { 111 | return (float)readPressureRaw() / 4096; 112 | } 113 | 114 | // reads pressure in inches of mercury (inHg) 115 | float LPS::readPressureInchesHg(void) 116 | { 117 | return (float)readPressureRaw() / 138706.5; 118 | } 119 | 120 | // reads pressure and returns raw 24-bit sensor output 121 | int32_t LPS::readPressureRaw(void) 122 | { 123 | Wire.beginTransmission(address); 124 | // assert MSB to enable register address auto-increment 125 | Wire.write(PRESS_OUT_XL | (1 << 7)); 126 | Wire.endTransmission(); 127 | Wire.requestFrom(address, (byte)3); 128 | 129 | while (Wire.available() < 3); 130 | 131 | uint8_t pxl = Wire.read(); 132 | uint8_t pl = Wire.read(); 133 | uint8_t ph = Wire.read(); 134 | 135 | // combine bytes 136 | return (int32_t)(int8_t)ph << 16 | (uint16_t)pl << 8 | pxl; 137 | } 138 | 139 | // reads temperature in degrees C 140 | float LPS::readTemperatureC(void) 141 | { 142 | return 42.5 + (float)readTemperatureRaw() / 480; 143 | } 144 | 145 | // reads temperature in degrees F 146 | float LPS::readTemperatureF(void) 147 | { 148 | return 108.5 + (float)readTemperatureRaw() / 480 * 1.8; 149 | } 150 | 151 | // reads temperature and returns raw 16-bit sensor output 152 | int16_t LPS::readTemperatureRaw(void) 153 | { 154 | Wire.beginTransmission(address); 155 | // assert MSB to enable register address auto-increment 156 | Wire.write(TEMP_OUT_L | (1 << 7)); 157 | Wire.endTransmission(); 158 | Wire.requestFrom(address, (byte)2); 159 | 160 | while (Wire.available() < 2); 161 | 162 | uint8_t tl = Wire.read(); 163 | uint8_t th = Wire.read(); 164 | 165 | // combine bytes 166 | return (int16_t)(th << 8 | tl); 167 | } 168 | 169 | // converts pressure in mbar to altitude in meters, using 1976 US 170 | // Standard Atmosphere model (note that this formula only applies to a 171 | // height of 11 km, or about 36000 ft) 172 | // If altimeter setting (QNH, barometric pressure adjusted to sea 173 | // level) is given, this function returns an indicated altitude 174 | // compensated for actual regional pressure; otherwise, it returns 175 | // the pressure altitude above the standard pressure level of 1013.25 176 | // mbar or 29.9213 inHg 177 | float LPS::pressureToAltitudeMeters(float pressure_mbar, float altimeter_setting_mbar) 178 | { 179 | return (1 - pow(pressure_mbar / altimeter_setting_mbar, 0.190263)) * 44330.8; 180 | } 181 | 182 | // converts pressure in inHg to altitude in feet; see notes above 183 | float LPS::pressureToAltitudeFeet(float pressure_inHg, float altimeter_setting_inHg) 184 | { 185 | return (1 - pow(pressure_inHg / altimeter_setting_inHg, 0.190263)) * 145442; 186 | } 187 | 188 | // Private Methods /////////////////////////////////////////////////// 189 | 190 | bool LPS::detectDeviceAndAddress(deviceType device, sa0State sa0) 191 | { 192 | if (sa0 == sa0_auto || sa0 == sa0_high) 193 | { 194 | address = SA0_HIGH_ADDRESS; 195 | if (detectDevice(device)) return true; 196 | } 197 | if (sa0 == sa0_auto || sa0 == sa0_low) 198 | { 199 | address = SA0_LOW_ADDRESS; 200 | if (detectDevice(device)) return true; 201 | } 202 | 203 | return false; 204 | } 205 | 206 | bool LPS::detectDevice(deviceType device) 207 | { 208 | int id = testWhoAmI(address); 209 | 210 | if ((device == device_auto || device == device_25H) && id == LPS25H_WHO_ID) 211 | { 212 | _device = device_25H; 213 | return true; 214 | } 215 | if ((device == device_auto || device == device_331AP) && id == LPS331AP_WHO_ID) 216 | { 217 | _device = device_331AP; 218 | return true; 219 | } 220 | 221 | return false; 222 | } 223 | 224 | int LPS::testWhoAmI(byte address) 225 | { 226 | Wire.beginTransmission(address); 227 | Wire.write(WHO_AM_I); 228 | Wire.endTransmission(); 229 | 230 | Wire.requestFrom(address, (byte)1); 231 | if (Wire.available()) 232 | return Wire.read(); 233 | else 234 | return TEST_REG_NACK; 235 | } -------------------------------------------------------------------------------- /libraries/IMU/LPS.h: -------------------------------------------------------------------------------- 1 | #ifndef LPS_h 2 | #define LPS_h 3 | 4 | #include // for byte data type 5 | 6 | class LPS 7 | { 8 | public: 9 | enum deviceType { device_331AP, device_25H, device_auto }; 10 | enum sa0State { sa0_low, sa0_high, sa0_auto }; 11 | 12 | // register addresses 13 | // Note: where register names differ between the register mapping table and 14 | // the register descriptions in the datasheets, the names from the register 15 | // descriptions are used here. 16 | enum regAddr 17 | { 18 | REF_P_XL = 0x08, 19 | REF_P_L = 0x09, 20 | REF_P_H = 0x0A, 21 | 22 | WHO_AM_I = 0x0F, 23 | 24 | RES_CONF = 0x10, 25 | 26 | CTRL_REG1 = 0x20, 27 | CTRL_REG2 = 0x21, 28 | CTRL_REG3 = 0x22, 29 | CTRL_REG4 = 0x23, // 25H 30 | 31 | STATUS_REG = 0x27, 32 | 33 | PRESS_OUT_XL = 0x28, 34 | PRESS_OUT_L = 0x29, 35 | PRESS_OUT_H = 0x2A, 36 | 37 | TEMP_OUT_L = 0x2B, 38 | TEMP_OUT_H = 0x2C, 39 | 40 | FIFO_CTRL = 0x2E, // 25H 41 | FIFO_STATUS = 0x2F, // 25H 42 | 43 | AMP_CTRL = 0x30, // 331AP 44 | 45 | RPDS_L = 0x39, // 25H 46 | RPDS_H = 0x3A, // 25H 47 | 48 | DELTA_PRESS_XL = 0x3C, // 331AP 49 | DELTA_PRESS_L = 0x3D, // 331AP 50 | DELTA_PRESS_H = 0x3E, // 331AP 51 | 52 | 53 | // dummy addresses for registers in different locations on different devices; 54 | // the library translates these based on device type 55 | // value with sign flipped is used as index into translated_regs array 56 | 57 | INTERRUPT_CFG = -1, 58 | INT_SOURCE = -2, 59 | THS_P_L = -3, 60 | THS_P_H = -4, 61 | // update dummy_reg_count if registers are added here! 62 | 63 | 64 | // device-specific register addresses 65 | 66 | LPS331AP_INTERRUPT_CFG = 0x23, 67 | LPS331AP_INT_SOURCE = 0x24, 68 | LPS331AP_THS_P_L = 0x25, 69 | LPS331AP_THS_P_H = 0x26, 70 | 71 | LPS25H_INTERRUPT_CFG = 0x24, 72 | LPS25H_INT_SOURCE = 0x25, 73 | LPS25H_THS_P_L = 0x30, 74 | LPS25H_THS_P_H = 0x31, 75 | }; 76 | 77 | LPS(void); 78 | 79 | bool init(deviceType device = device_auto, byte sa0 = sa0_auto); 80 | deviceType getDeviceType(void) { return _device; } 81 | byte getAddress(void) { return address; } 82 | 83 | void enableDefault(void); 84 | 85 | void writeReg(int reg, byte value); 86 | byte readReg(int reg); 87 | 88 | float readPressureMillibars(void); 89 | float readPressureInchesHg(void); 90 | int32_t readPressureRaw(void); 91 | float readTemperatureC(void); 92 | float readTemperatureF(void); 93 | int16_t readTemperatureRaw(void); 94 | 95 | static float pressureToAltitudeMeters(float pressure_mbar, float altimeter_setting_mbar = 1013.25); 96 | static float pressureToAltitudeFeet(float pressure_inHg, float altimeter_setting_inHg = 29.9213); 97 | 98 | private: 99 | deviceType _device; // chip type (331AP or 25H) 100 | byte address; 101 | 102 | static const int dummy_reg_count = 4; 103 | regAddr translated_regs[dummy_reg_count + 1]; // index 0 not used 104 | 105 | bool detectDeviceAndAddress(deviceType device, sa0State sa0); 106 | bool detectDevice(deviceType device); 107 | int testWhoAmI(byte address); 108 | }; 109 | 110 | #endif -------------------------------------------------------------------------------- /libraries/IMU/LSM303.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // Defines //////////////////////////////////////////////////////////////// 6 | 7 | // The Arduino two-wire interface uses a 7-bit number for the address, 8 | // and sets the last bit correctly based on reads and writes 9 | #define D_SA0_HIGH_ADDRESS 0b0011101 10 | #define D_SA0_LOW_ADDRESS 0b0011110 11 | #define DLHC_DLM_DLH_MAG_ADDRESS 0b0011110 12 | #define DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS 0b0011001 13 | #define DLM_DLH_ACC_SA0_LOW_ADDRESS 0b0011000 14 | 15 | #define TEST_REG_ERROR -1 16 | 17 | #define D_WHO_ID 0x49 18 | #define DLM_WHO_ID 0x3C 19 | 20 | // Constructors //////////////////////////////////////////////////////////////// 21 | 22 | LSM303::LSM303(void) 23 | { 24 | /* 25 | These values lead to an assumed magnetometer bias of 0. 26 | Use the Calibrate example program to determine appropriate values 27 | for your particular unit. The Heading example demonstrates how to 28 | adjust these values in your own sketch. 29 | */ 30 | m_min = (LSM303::vector){-32767, -32767, -32767}; 31 | m_max = (LSM303::vector){+32767, +32767, +32767}; 32 | 33 | _device = device_auto; 34 | 35 | io_timeout = 0; // 0 = no timeout 36 | did_timeout = false; 37 | } 38 | 39 | // Public Methods ////////////////////////////////////////////////////////////// 40 | 41 | // Did a timeout occur in readAcc(), readMag(), or read() since the last call to timeoutOccurred()? 42 | bool LSM303::timeoutOccurred() 43 | { 44 | bool tmp = did_timeout; 45 | did_timeout = false; 46 | return tmp; 47 | } 48 | 49 | void LSM303::setTimeout(unsigned int timeout) 50 | { 51 | io_timeout = timeout; 52 | } 53 | 54 | unsigned int LSM303::getTimeout() 55 | { 56 | return io_timeout; 57 | } 58 | 59 | bool LSM303::init(deviceType device, sa0State sa0) 60 | { 61 | // perform auto-detection unless device type and SA0 state were both specified 62 | if (device == device_auto || sa0 == sa0_auto) 63 | { 64 | // check for LSM303D if device is unidentified or was specified to be this type 65 | if (device == device_auto || device == device_D) 66 | { 67 | // check SA0 high address unless SA0 was specified to be low 68 | if (sa0 != sa0_low && testReg(D_SA0_HIGH_ADDRESS, WHO_AM_I) == D_WHO_ID) 69 | { 70 | // device responds to address 0011101 with D ID; it's a D with SA0 high 71 | device = device_D; 72 | sa0 = sa0_high; 73 | } 74 | // check SA0 low address unless SA0 was specified to be high 75 | else if (sa0 != sa0_high && testReg(D_SA0_LOW_ADDRESS, WHO_AM_I) == D_WHO_ID) 76 | { 77 | // device responds to address 0011110 with D ID; it's a D with SA0 low 78 | device = device_D; 79 | sa0 = sa0_low; 80 | } 81 | } 82 | 83 | // check for LSM303DLHC, DLM, DLH if device is still unidentified or was specified to be one of these types 84 | if (device == device_auto || device == device_DLHC || device == device_DLM || device == device_DLH) 85 | { 86 | // check SA0 high address unless SA0 was specified to be low 87 | if (sa0 != sa0_low && testReg(DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR) 88 | { 89 | // device responds to address 0011001; it's a DLHC, DLM with SA0 high, or DLH with SA0 high 90 | sa0 = sa0_high; 91 | if (device == device_auto) 92 | { 93 | // use magnetometer WHO_AM_I register to determine device type 94 | // 95 | // DLHC seems to respond to WHO_AM_I request the same way as DLM, even though this 96 | // register isn't documented in its datasheet. Since the DLHC accelerometer address is the 97 | // same as the DLM with SA0 high, but Pololu DLM boards pull SA0 low by default, we'll 98 | // guess that a device whose accelerometer responds to the SA0 high address and whose 99 | // magnetometer gives the DLM ID is actually a DLHC. 100 | device = (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID) ? device_DLHC : device_DLH; 101 | } 102 | } 103 | // check SA0 low address unless SA0 was specified to be high 104 | else if (sa0 != sa0_high && testReg(DLM_DLH_ACC_SA0_LOW_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR) 105 | { 106 | // device responds to address 0011000; it's a DLM with SA0 low or DLH with SA0 low 107 | sa0 = sa0_low; 108 | if (device == device_auto) 109 | { 110 | // use magnetometer WHO_AM_I register to determine device type 111 | device = (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID) ? device_DLM : device_DLH; 112 | } 113 | } 114 | } 115 | 116 | // make sure device and SA0 were successfully detected; otherwise, indicate failure 117 | if (device == device_auto || sa0 == sa0_auto) 118 | { 119 | return false; 120 | } 121 | } 122 | 123 | _device = device; 124 | 125 | // set device addresses and translated register addresses 126 | switch (device) 127 | { 128 | case device_D: 129 | acc_address = mag_address = (sa0 == sa0_high) ? D_SA0_HIGH_ADDRESS : D_SA0_LOW_ADDRESS; 130 | translated_regs[-OUT_X_L_M] = D_OUT_X_L_M; 131 | translated_regs[-OUT_X_H_M] = D_OUT_X_H_M; 132 | translated_regs[-OUT_Y_L_M] = D_OUT_Y_L_M; 133 | translated_regs[-OUT_Y_H_M] = D_OUT_Y_H_M; 134 | translated_regs[-OUT_Z_L_M] = D_OUT_Z_L_M; 135 | translated_regs[-OUT_Z_H_M] = D_OUT_Z_H_M; 136 | break; 137 | 138 | case device_DLHC: 139 | acc_address = DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS; // DLHC doesn't have configurable SA0 but uses same acc address as DLM/DLH with SA0 high 140 | mag_address = DLHC_DLM_DLH_MAG_ADDRESS; 141 | translated_regs[-OUT_X_H_M] = DLHC_OUT_X_H_M; 142 | translated_regs[-OUT_X_L_M] = DLHC_OUT_X_L_M; 143 | translated_regs[-OUT_Y_H_M] = DLHC_OUT_Y_H_M; 144 | translated_regs[-OUT_Y_L_M] = DLHC_OUT_Y_L_M; 145 | translated_regs[-OUT_Z_H_M] = DLHC_OUT_Z_H_M; 146 | translated_regs[-OUT_Z_L_M] = DLHC_OUT_Z_L_M; 147 | break; 148 | 149 | case device_DLM: 150 | acc_address = (sa0 == sa0_high) ? DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS : DLM_DLH_ACC_SA0_LOW_ADDRESS; 151 | mag_address = DLHC_DLM_DLH_MAG_ADDRESS; 152 | translated_regs[-OUT_X_H_M] = DLM_OUT_X_H_M; 153 | translated_regs[-OUT_X_L_M] = DLM_OUT_X_L_M; 154 | translated_regs[-OUT_Y_H_M] = DLM_OUT_Y_H_M; 155 | translated_regs[-OUT_Y_L_M] = DLM_OUT_Y_L_M; 156 | translated_regs[-OUT_Z_H_M] = DLM_OUT_Z_H_M; 157 | translated_regs[-OUT_Z_L_M] = DLM_OUT_Z_L_M; 158 | break; 159 | 160 | case device_DLH: 161 | acc_address = (sa0 == sa0_high) ? DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS : DLM_DLH_ACC_SA0_LOW_ADDRESS; 162 | mag_address = DLHC_DLM_DLH_MAG_ADDRESS; 163 | translated_regs[-OUT_X_H_M] = DLH_OUT_X_H_M; 164 | translated_regs[-OUT_X_L_M] = DLH_OUT_X_L_M; 165 | translated_regs[-OUT_Y_H_M] = DLH_OUT_Y_H_M; 166 | translated_regs[-OUT_Y_L_M] = DLH_OUT_Y_L_M; 167 | translated_regs[-OUT_Z_H_M] = DLH_OUT_Z_H_M; 168 | translated_regs[-OUT_Z_L_M] = DLH_OUT_Z_L_M; 169 | break; 170 | } 171 | 172 | return true; 173 | } 174 | 175 | /* 176 | Enables the LSM303's accelerometer and magnetometer. Also: 177 | - Sets sensor full scales (gain) to default power-on values, which are 178 | +/- 2 g for accelerometer and +/- 1.3 gauss for magnetometer 179 | (+/- 4 gauss on LSM303D). 180 | - Selects 50 Hz ODR (output data rate) for accelerometer and 7.5 Hz 181 | ODR for magnetometer (6.25 Hz on LSM303D). (These are the ODR 182 | settings for which the electrical characteristics are specified in 183 | the datasheets.) 184 | - Enables high resolution modes (if available). 185 | Note that this function will also reset other settings controlled by 186 | the registers it writes to. 187 | */ 188 | void LSM303::enableDefault(void) 189 | { 190 | 191 | if (_device == device_D) 192 | { 193 | // Accelerometer 194 | 195 | // 0x00 = 0b00000000 196 | // AFS = 0 (+/- 2 g full scale) 197 | writeReg(CTRL2, 0x00); 198 | 199 | // 0x57 = 0b01010111 200 | // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled) 201 | writeReg(CTRL1, 0x57); 202 | 203 | // Magnetometer 204 | 205 | // 0x64 = 0b01100100 206 | // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR) 207 | writeReg(CTRL5, 0x64); 208 | 209 | // 0x20 = 0b00100000 210 | // MFS = 01 (+/- 4 gauss full scale) 211 | writeReg(CTRL6, 0x20); 212 | 213 | // 0x00 = 0b00000000 214 | // MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode) 215 | writeReg(CTRL7, 0x00); 216 | } 217 | else 218 | { 219 | // Accelerometer 220 | 221 | if (_device == device_DLHC) 222 | { 223 | // 0x08 = 0b00001000 224 | // FS = 00 (+/- 2 g full scale); HR = 1 (high resolution enable) 225 | writeAccReg(CTRL_REG4_A, 0x08); 226 | 227 | // 0x47 = 0b01000111 228 | // ODR = 0100 (50 Hz ODR); LPen = 0 (normal mode); Zen = Yen = Xen = 1 (all axes enabled) 229 | writeAccReg(CTRL_REG1_A, 0x47); 230 | } 231 | else // DLM, DLH 232 | { 233 | // 0x00 = 0b00000000 234 | // FS = 00 (+/- 2 g full scale) 235 | writeAccReg(CTRL_REG4_A, 0x00); 236 | 237 | // 0x27 = 0b00100111 238 | // PM = 001 (normal mode); DR = 00 (50 Hz ODR); Zen = Yen = Xen = 1 (all axes enabled) 239 | writeAccReg(CTRL_REG1_A, 0x27); 240 | } 241 | 242 | // Magnetometer 243 | 244 | // 0x0C = 0b00001100 245 | // DO = 011 (7.5 Hz ODR) 246 | writeMagReg(CRA_REG_M, 0x0C); 247 | 248 | // 0x20 = 0b00100000 249 | // GN = 001 (+/- 1.3 gauss full scale) 250 | writeMagReg(CRB_REG_M, 0x20); 251 | 252 | // 0x00 = 0b00000000 253 | // MD = 00 (continuous-conversion mode) 254 | writeMagReg(MR_REG_M, 0x00); 255 | } 256 | } 257 | 258 | // Writes an accelerometer register 259 | void LSM303::writeAccReg(byte reg, byte value) 260 | { 261 | Wire.beginTransmission(acc_address); 262 | Wire.write(reg); 263 | Wire.write(value); 264 | last_status = Wire.endTransmission(); 265 | } 266 | 267 | // Reads an accelerometer register 268 | byte LSM303::readAccReg(byte reg) 269 | { 270 | byte value; 271 | 272 | Wire.beginTransmission(acc_address); 273 | Wire.write(reg); 274 | last_status = Wire.endTransmission(); 275 | Wire.requestFrom(acc_address, (byte)1); 276 | value = Wire.read(); 277 | Wire.endTransmission(); 278 | 279 | return value; 280 | } 281 | 282 | // Writes a magnetometer register 283 | void LSM303::writeMagReg(byte reg, byte value) 284 | { 285 | Wire.beginTransmission(mag_address); 286 | Wire.write(reg); 287 | Wire.write(value); 288 | last_status = Wire.endTransmission(); 289 | } 290 | 291 | // Reads a magnetometer register 292 | byte LSM303::readMagReg(int reg) 293 | { 294 | byte value; 295 | 296 | // if dummy register address (magnetometer Y/Z), look up actual translated address (based on device type) 297 | if (reg < 0) 298 | { 299 | reg = translated_regs[-reg]; 300 | } 301 | 302 | Wire.beginTransmission(mag_address); 303 | Wire.write(reg); 304 | last_status = Wire.endTransmission(); 305 | Wire.requestFrom(mag_address, (byte)1); 306 | value = Wire.read(); 307 | Wire.endTransmission(); 308 | 309 | return value; 310 | } 311 | 312 | void LSM303::writeReg(byte reg, byte value) 313 | { 314 | // mag address == acc_address for LSM303D, so it doesn't really matter which one we use. 315 | if (_device == device_D || reg < CTRL_REG1_A) 316 | { 317 | writeMagReg(reg, value); 318 | } 319 | else 320 | { 321 | writeAccReg(reg, value); 322 | } 323 | } 324 | 325 | // Note that this function will not work for reading TEMP_OUT_H_M and TEMP_OUT_L_M on the DLHC. 326 | // To read those two registers, use readMagReg() instead. 327 | byte LSM303::readReg(int reg) 328 | { 329 | // mag address == acc_address for LSM303D, so it doesn't really matter which one we use. 330 | // Use readMagReg so it can translate OUT_[XYZ]_[HL]_M 331 | if (_device == device_D || reg < CTRL_REG1_A) 332 | { 333 | return readMagReg(reg); 334 | } 335 | else 336 | { 337 | return readAccReg(reg); 338 | } 339 | } 340 | 341 | // Reads the 3 accelerometer channels and stores them in vector a 342 | void LSM303::readAcc(void) 343 | { 344 | Wire.beginTransmission(acc_address); 345 | // assert the MSB of the address to get the accelerometer 346 | // to do slave-transmit subaddress updating. 347 | Wire.write(OUT_X_L_A | (1 << 7)); 348 | last_status = Wire.endTransmission(); 349 | Wire.requestFrom(acc_address, (byte)6); 350 | 351 | unsigned int millis_start = millis(); 352 | while (Wire.available() < 6) { 353 | if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout) 354 | { 355 | did_timeout = true; 356 | return; 357 | } 358 | } 359 | 360 | byte xla = Wire.read(); 361 | byte xha = Wire.read(); 362 | byte yla = Wire.read(); 363 | byte yha = Wire.read(); 364 | byte zla = Wire.read(); 365 | byte zha = Wire.read(); 366 | 367 | // combine high and low bytes 368 | // This no longer drops the lowest 4 bits of the readings from the DLH/DLM/DLHC, which are always 0 369 | // (12-bit resolution, left-aligned). The D has 16-bit resolution 370 | a.x = (int16_t)(xha << 8 | xla); 371 | a.y = (int16_t)(yha << 8 | yla); 372 | a.z = (int16_t)(zha << 8 | zla); 373 | } 374 | 375 | // Reads the 3 magnetometer channels and stores them in vector m 376 | void LSM303::readMag(void) 377 | { 378 | Wire.beginTransmission(mag_address); 379 | // If LSM303D, assert MSB to enable subaddress updating 380 | // OUT_X_L_M comes first on D, OUT_X_H_M on others 381 | Wire.write((_device == device_D) ? translated_regs[-OUT_X_L_M] | (1 << 7) : translated_regs[-OUT_X_H_M]); 382 | last_status = Wire.endTransmission(); 383 | Wire.requestFrom(mag_address, (byte)6); 384 | 385 | unsigned int millis_start = millis(); 386 | while (Wire.available() < 6) { 387 | if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout) 388 | { 389 | did_timeout = true; 390 | return; 391 | } 392 | } 393 | 394 | byte xlm, xhm, ylm, yhm, zlm, zhm; 395 | 396 | if (_device == device_D) 397 | { 398 | // D: X_L, X_H, Y_L, Y_H, Z_L, Z_H 399 | xlm = Wire.read(); 400 | xhm = Wire.read(); 401 | ylm = Wire.read(); 402 | yhm = Wire.read(); 403 | zlm = Wire.read(); 404 | zhm = Wire.read(); 405 | } 406 | else 407 | { 408 | // DLHC, DLM, DLH: X_H, X_L... 409 | xhm = Wire.read(); 410 | xlm = Wire.read(); 411 | 412 | if (_device == device_DLH) 413 | { 414 | // DLH: ...Y_H, Y_L, Z_H, Z_L 415 | yhm = Wire.read(); 416 | ylm = Wire.read(); 417 | zhm = Wire.read(); 418 | zlm = Wire.read(); 419 | } 420 | else 421 | { 422 | // DLM, DLHC: ...Z_H, Z_L, Y_H, Y_L 423 | zhm = Wire.read(); 424 | zlm = Wire.read(); 425 | yhm = Wire.read(); 426 | ylm = Wire.read(); 427 | } 428 | } 429 | 430 | // combine high and low bytes 431 | m.x = (int16_t)(xhm << 8 | xlm); 432 | m.y = (int16_t)(yhm << 8 | ylm); 433 | m.z = (int16_t)(zhm << 8 | zlm); 434 | } 435 | 436 | // Reads all 6 channels of the LSM303 and stores them in the object variables 437 | void LSM303::read(void) 438 | { 439 | readAcc(); 440 | readMag(); 441 | } 442 | 443 | /* 444 | Returns the angular difference in the horizontal plane between a 445 | default vector and north, in degrees. 446 | 447 | The default vector here is chosen to point along the surface of the 448 | PCB, in the direction of the top of the text on the silkscreen. 449 | This is the +X axis on the Pololu LSM303D carrier and the -Y axis on 450 | the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH carriers. 451 | */ 452 | float LSM303::heading(void) 453 | { 454 | if (_device == device_D) 455 | { 456 | return heading((vector){1, 0, 0}); 457 | } 458 | else 459 | { 460 | return heading((vector){0, -1, 0}); 461 | } 462 | } 463 | 464 | void LSM303::vector_normalize(vector *a) 465 | { 466 | float mag = sqrt(vector_dot(a, a)); 467 | a->x /= mag; 468 | a->y /= mag; 469 | a->z /= mag; 470 | } 471 | 472 | // Private Methods ////////////////////////////////////////////////////////////// 473 | 474 | int LSM303::testReg(byte address, regAddr reg) 475 | { 476 | Wire.beginTransmission(address); 477 | Wire.write((byte)reg); 478 | if (Wire.endTransmission() != 0) 479 | { 480 | return TEST_REG_ERROR; 481 | } 482 | 483 | Wire.requestFrom(address, (byte)1); 484 | if (Wire.available()) 485 | { 486 | return Wire.read(); 487 | } 488 | else 489 | { 490 | return TEST_REG_ERROR; 491 | } 492 | } -------------------------------------------------------------------------------- /libraries/IMU/LSM303.h: -------------------------------------------------------------------------------- 1 | #ifndef LSM303_h 2 | #define LSM303_h 3 | 4 | #include // for byte data type 5 | 6 | class LSM303 7 | { 8 | public: 9 | template struct vector 10 | { 11 | T x, y, z; 12 | }; 13 | 14 | enum deviceType { device_DLH, device_DLM, device_DLHC, device_D, device_auto }; 15 | enum sa0State { sa0_low, sa0_high, sa0_auto }; 16 | 17 | // register addresses 18 | enum regAddr 19 | { 20 | TEMP_OUT_L = 0x05, // D 21 | TEMP_OUT_H = 0x06, // D 22 | 23 | STATUS_M = 0x07, // D 24 | 25 | INT_CTRL_M = 0x12, // D 26 | INT_SRC_M = 0x13, // D 27 | INT_THS_L_M = 0x14, // D 28 | INT_THS_H_M = 0x15, // D 29 | 30 | OFFSET_X_L_M = 0x16, // D 31 | OFFSET_X_H_M = 0x17, // D 32 | OFFSET_Y_L_M = 0x18, // D 33 | OFFSET_Y_H_M = 0x19, // D 34 | OFFSET_Z_L_M = 0x1A, // D 35 | OFFSET_Z_H_M = 0x1B, // D 36 | REFERENCE_X = 0x1C, // D 37 | REFERENCE_Y = 0x1D, // D 38 | REFERENCE_Z = 0x1E, // D 39 | 40 | CTRL0 = 0x1F, // D 41 | CTRL1 = 0x20, // D 42 | CTRL_REG1_A = 0x20, // DLH, DLM, DLHC 43 | CTRL2 = 0x21, // D 44 | CTRL_REG2_A = 0x21, // DLH, DLM, DLHC 45 | CTRL3 = 0x22, // D 46 | CTRL_REG3_A = 0x22, // DLH, DLM, DLHC 47 | CTRL4 = 0x23, // D 48 | CTRL_REG4_A = 0x23, // DLH, DLM, DLHC 49 | CTRL5 = 0x24, // D 50 | CTRL_REG5_A = 0x24, // DLH, DLM, DLHC 51 | CTRL6 = 0x25, // D 52 | CTRL_REG6_A = 0x25, // DLHC 53 | HP_FILTER_RESET_A = 0x25, // DLH, DLM 54 | CTRL7 = 0x26, // D 55 | REFERENCE_A = 0x26, // DLH, DLM, DLHC 56 | STATUS_A = 0x27, // D 57 | STATUS_REG_A = 0x27, // DLH, DLM, DLHC 58 | 59 | OUT_X_L_A = 0x28, 60 | OUT_X_H_A = 0x29, 61 | OUT_Y_L_A = 0x2A, 62 | OUT_Y_H_A = 0x2B, 63 | OUT_Z_L_A = 0x2C, 64 | OUT_Z_H_A = 0x2D, 65 | 66 | FIFO_CTRL = 0x2E, // D 67 | FIFO_CTRL_REG_A = 0x2E, // DLHC 68 | FIFO_SRC = 0x2F, // D 69 | FIFO_SRC_REG_A = 0x2F, // DLHC 70 | 71 | IG_CFG1 = 0x30, // D 72 | INT1_CFG_A = 0x30, // DLH, DLM, DLHC 73 | IG_SRC1 = 0x31, // D 74 | INT1_SRC_A = 0x31, // DLH, DLM, DLHC 75 | IG_THS1 = 0x32, // D 76 | INT1_THS_A = 0x32, // DLH, DLM, DLHC 77 | IG_DUR1 = 0x33, // D 78 | INT1_DURATION_A = 0x33, // DLH, DLM, DLHC 79 | IG_CFG2 = 0x34, // D 80 | INT2_CFG_A = 0x34, // DLH, DLM, DLHC 81 | IG_SRC2 = 0x35, // D 82 | INT2_SRC_A = 0x35, // DLH, DLM, DLHC 83 | IG_THS2 = 0x36, // D 84 | INT2_THS_A = 0x36, // DLH, DLM, DLHC 85 | IG_DUR2 = 0x37, // D 86 | INT2_DURATION_A = 0x37, // DLH, DLM, DLHC 87 | 88 | CLICK_CFG = 0x38, // D 89 | CLICK_CFG_A = 0x38, // DLHC 90 | CLICK_SRC = 0x39, // D 91 | CLICK_SRC_A = 0x39, // DLHC 92 | CLICK_THS = 0x3A, // D 93 | CLICK_THS_A = 0x3A, // DLHC 94 | TIME_LIMIT = 0x3B, // D 95 | TIME_LIMIT_A = 0x3B, // DLHC 96 | TIME_LATENCY = 0x3C, // D 97 | TIME_LATENCY_A = 0x3C, // DLHC 98 | TIME_WINDOW = 0x3D, // D 99 | TIME_WINDOW_A = 0x3D, // DLHC 100 | 101 | Act_THS = 0x3E, // D 102 | Act_DUR = 0x3F, // D 103 | 104 | CRA_REG_M = 0x00, // DLH, DLM, DLHC 105 | CRB_REG_M = 0x01, // DLH, DLM, DLHC 106 | MR_REG_M = 0x02, // DLH, DLM, DLHC 107 | 108 | SR_REG_M = 0x09, // DLH, DLM, DLHC 109 | IRA_REG_M = 0x0A, // DLH, DLM, DLHC 110 | IRB_REG_M = 0x0B, // DLH, DLM, DLHC 111 | IRC_REG_M = 0x0C, // DLH, DLM, DLHC 112 | 113 | WHO_AM_I = 0x0F, // D 114 | WHO_AM_I_M = 0x0F, // DLM 115 | 116 | TEMP_OUT_H_M = 0x31, // DLHC 117 | TEMP_OUT_L_M = 0x32, // DLHC 118 | 119 | 120 | // dummy addresses for registers in different locations on different devices; 121 | // the library translates these based on device type 122 | // value with sign flipped is used as index into translated_regs array 123 | 124 | OUT_X_H_M = -1, 125 | OUT_X_L_M = -2, 126 | OUT_Y_H_M = -3, 127 | OUT_Y_L_M = -4, 128 | OUT_Z_H_M = -5, 129 | OUT_Z_L_M = -6, 130 | // update dummy_reg_count if registers are added here! 131 | 132 | // device-specific register addresses 133 | 134 | DLH_OUT_X_H_M = 0x03, 135 | DLH_OUT_X_L_M = 0x04, 136 | DLH_OUT_Y_H_M = 0x05, 137 | DLH_OUT_Y_L_M = 0x06, 138 | DLH_OUT_Z_H_M = 0x07, 139 | DLH_OUT_Z_L_M = 0x08, 140 | 141 | DLM_OUT_X_H_M = 0x03, 142 | DLM_OUT_X_L_M = 0x04, 143 | DLM_OUT_Z_H_M = 0x05, 144 | DLM_OUT_Z_L_M = 0x06, 145 | DLM_OUT_Y_H_M = 0x07, 146 | DLM_OUT_Y_L_M = 0x08, 147 | 148 | DLHC_OUT_X_H_M = 0x03, 149 | DLHC_OUT_X_L_M = 0x04, 150 | DLHC_OUT_Z_H_M = 0x05, 151 | DLHC_OUT_Z_L_M = 0x06, 152 | DLHC_OUT_Y_H_M = 0x07, 153 | DLHC_OUT_Y_L_M = 0x08, 154 | 155 | D_OUT_X_L_M = 0x08, 156 | D_OUT_X_H_M = 0x09, 157 | D_OUT_Y_L_M = 0x0A, 158 | D_OUT_Y_H_M = 0x0B, 159 | D_OUT_Z_L_M = 0x0C, 160 | D_OUT_Z_H_M = 0x0D 161 | }; 162 | 163 | vector a; // accelerometer readings 164 | vector m; // magnetometer readings 165 | vector m_max; // maximum magnetometer values, used for calibration 166 | vector m_min; // minimum magnetometer values, used for calibration 167 | 168 | byte last_status; // status of last I2C transmission 169 | 170 | LSM303(void); 171 | 172 | bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto); 173 | deviceType getDeviceType(void) { return _device; } 174 | 175 | void enableDefault(void); 176 | 177 | void writeAccReg(byte reg, byte value); 178 | byte readAccReg(byte reg); 179 | void writeMagReg(byte reg, byte value); 180 | byte readMagReg(int reg); 181 | 182 | void writeReg(byte reg, byte value); 183 | byte readReg(int reg); 184 | 185 | void readAcc(void); 186 | void readMag(void); 187 | void read(void); 188 | 189 | void setTimeout(unsigned int timeout); 190 | unsigned int getTimeout(void); 191 | bool timeoutOccurred(void); 192 | 193 | float heading(void); 194 | template float heading(vector from); 195 | 196 | // vector functions 197 | template static void vector_cross(const vector *a, const vector *b, vector *out); 198 | template static float vector_dot(const vector *a, const vector *b); 199 | static void vector_normalize(vector *a); 200 | 201 | private: 202 | deviceType _device; // chip type (D, DLHC, DLM, or DLH) 203 | byte acc_address; 204 | byte mag_address; 205 | 206 | static const int dummy_reg_count = 6; 207 | regAddr translated_regs[dummy_reg_count + 1]; // index 0 not used 208 | 209 | unsigned int io_timeout; 210 | bool did_timeout; 211 | 212 | int testReg(byte address, regAddr reg); 213 | }; 214 | 215 | /* 216 | Returns the angular difference in the horizontal plane between the 217 | "from" vector and north, in degrees. 218 | 219 | Description of heading algorithm: 220 | Shift and scale the magnetic reading based on calibration data to find 221 | the North vector. Use the acceleration readings to determine the Up 222 | vector (gravity is measured as an upward acceleration). The cross 223 | product of North and Up vectors is East. The vectors East and North 224 | form a basis for the horizontal plane. The From vector is projected 225 | into the horizontal plane and the angle between the projected vector 226 | and horizontal north is returned. 227 | */ 228 | template float LSM303::heading(vector from) 229 | { 230 | vector temp_m = {m.x, m.y, m.z}; 231 | 232 | // subtract offset (average of min and max) from magnetometer readings 233 | temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2; 234 | temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2; 235 | temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2; 236 | 237 | // compute E and N 238 | vector E; 239 | vector N; 240 | vector_cross(&temp_m, &a, &E); 241 | vector_normalize(&E); 242 | vector_cross(&a, &E, &N); 243 | vector_normalize(&N); 244 | 245 | // compute heading 246 | float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI; 247 | if (heading < 0) heading += 360; 248 | return heading; 249 | } 250 | 251 | template void LSM303::vector_cross(const vector *a, const vector *b, vector *out) 252 | { 253 | out->x = (a->y * b->z) - (a->z * b->y); 254 | out->y = (a->z * b->x) - (a->x * b->z); 255 | out->z = (a->x * b->y) - (a->y * b->x); 256 | } 257 | 258 | template float LSM303::vector_dot(const vector *a, const vector *b) 259 | { 260 | return (a->x * b->x) + (a->y * b->y) + (a->z * b->z); 261 | } 262 | 263 | #endif 264 | 265 | 266 | 267 | -------------------------------------------------------------------------------- /libraries/Movement/Movement.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /** 4 | * Constructor args are pins for left and right motor speed and direction 5 | **/ 6 | Movement::Movement(byte rightSpeedPin, byte rightDirectionA, byte rightDirectionB, byte leftSpeedPin, byte leftDirectionA, byte leftDirectionB) { 7 | pinMode(rightSpeedPin, OUTPUT); 8 | pinMode(rightDirectionA, OUTPUT); 9 | pinMode(rightDirectionB, OUTPUT); 10 | pinMode(leftSpeedPin, OUTPUT); 11 | pinMode(leftDirectionA, OUTPUT); 12 | pinMode(leftDirectionB, OUTPUT); 13 | _rightSpeedPin = rightSpeedPin; 14 | _rightDirectionA = rightDirectionA; 15 | _rightDirectionB = rightDirectionB; 16 | _leftSpeedPin = leftSpeedPin; 17 | _leftDirectionA = leftDirectionA; 18 | _leftDirectionB = leftDirectionB; 19 | } 20 | 21 | /** 22 | * Sets forward motion at speed, args are for left and right tread speed 23 | **/ 24 | void Movement::forward(byte leftSpeed, byte rightSpeed) { 25 | digitalWrite(_leftDirectionA, LOW); 26 | digitalWrite(_leftDirectionB, HIGH); 27 | analogWrite(_leftSpeedPin, leftSpeed); 28 | digitalWrite(_rightDirectionA, HIGH); 29 | digitalWrite(_rightDirectionB, LOW); 30 | analogWrite(_rightSpeedPin, rightSpeed); 31 | } 32 | 33 | /** 34 | * Sets backward motion at speed, args are for left and right tread speed 35 | **/ 36 | void Movement::backward(byte leftSpeed, byte rightSpeed) { 37 | digitalWrite(_leftDirectionA, HIGH); 38 | digitalWrite(_leftDirectionB, LOW); 39 | analogWrite(_leftSpeedPin, leftSpeed); 40 | digitalWrite(_rightDirectionA, LOW); 41 | digitalWrite(_rightDirectionB, HIGH); 42 | analogWrite(_rightSpeedPin, rightSpeed); 43 | } 44 | 45 | /** 46 | * Rotates right at speed. Treads will move in opposite directions 47 | **/ 48 | void Movement::rotateRight(byte leftSpeed, byte rightSpeed) { 49 | digitalWrite(_leftDirectionA, LOW); 50 | digitalWrite(_leftDirectionB, HIGH); 51 | analogWrite(_leftSpeedPin, leftSpeed); 52 | digitalWrite(_rightDirectionA, LOW); 53 | digitalWrite(_rightDirectionB, HIGH); 54 | analogWrite(_rightSpeedPin, rightSpeed); 55 | } 56 | 57 | /** 58 | * Rotates left at speed. Treads will move in opposite directions 59 | **/ 60 | void Movement::rotateLeft(byte leftSpeed, byte rightSpeed) { 61 | digitalWrite(_leftDirectionA, HIGH); 62 | digitalWrite(_leftDirectionB, LOW); 63 | analogWrite(_leftSpeedPin, leftSpeed); 64 | digitalWrite(_rightDirectionA, HIGH); 65 | digitalWrite(_rightDirectionB, LOW); 66 | analogWrite(_rightSpeedPin, rightSpeed); 67 | } 68 | 69 | /** 70 | * Stops all motor motion 71 | **/ 72 | void Movement::stop() { 73 | digitalWrite(_leftDirectionA, LOW); 74 | digitalWrite(_leftDirectionB, LOW); 75 | digitalWrite(_rightDirectionA, LOW); 76 | digitalWrite(_rightDirectionB, LOW); 77 | } 78 | -------------------------------------------------------------------------------- /libraries/Movement/Movement.h: -------------------------------------------------------------------------------- 1 | #ifndef Movement_h 2 | #define Movement_h 3 | 4 | #include "Arduino.h" 5 | 6 | class Movement 7 | { 8 | public: 9 | //Constructors 10 | Movement(byte rightSpeedPin, byte rightDirectionA, byte rightDirectionB, byte leftSpeedPin, byte leftDirectionA, byte leftDirectionB); 11 | 12 | //Functions 13 | void forward(byte leftSpeed, byte rightSpeed); 14 | void backward(byte leftSpeed, byte rightSpeed); 15 | void rotateRight(byte leftSpeed, byte rightSpeed); 16 | void rotateLeft(byte leftSpeed, byte rightSpeed); 17 | void stop(); 18 | 19 | private: 20 | byte _rightSpeedPin, _rightDirectionA, _rightDirectionB, _leftSpeedPin, _leftDirectionA, _leftDirectionB; 21 | }; 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /libraries/NewPing/NewPing.cpp: -------------------------------------------------------------------------------- 1 | // --------------------------------------------------------------------------- 2 | // Created by Tim Eckel - teckel@leethost.com 3 | // Copyright 2016 License: GNU GPL v3 http://www.gnu.org/licenses/gpl.html 4 | // 5 | // See "NewPing.h" for purpose, syntax, version history, links, and more. 6 | // --------------------------------------------------------------------------- 7 | 8 | #include "NewPing.h" 9 | 10 | 11 | // --------------------------------------------------------------------------- 12 | // NewPing constructor 13 | // --------------------------------------------------------------------------- 14 | 15 | NewPing::NewPing(uint8_t trigger_pin, uint8_t echo_pin, unsigned int max_cm_distance) { 16 | #if DO_BITWISE == true 17 | _triggerBit = digitalPinToBitMask(trigger_pin); // Get the port register bitmask for the trigger pin. 18 | _echoBit = digitalPinToBitMask(echo_pin); // Get the port register bitmask for the echo pin. 19 | 20 | _triggerOutput = portOutputRegister(digitalPinToPort(trigger_pin)); // Get the output port register for the trigger pin. 21 | _echoInput = portInputRegister(digitalPinToPort(echo_pin)); // Get the input port register for the echo pin. 22 | 23 | _triggerMode = (uint8_t *) portModeRegister(digitalPinToPort(trigger_pin)); // Get the port mode register for the trigger pin. 24 | #else 25 | _triggerPin = trigger_pin; 26 | _echoPin = echo_pin; 27 | #endif 28 | 29 | set_max_distance(max_cm_distance); // Call function to set the max sensor distance. 30 | 31 | #if (defined (__arm__) && defined (TEENSYDUINO)) || DO_BITWISE != true 32 | pinMode(echo_pin, INPUT); // Set echo pin to input (on Teensy 3.x (ARM), pins default to disabled, at least one pinMode() is needed for GPIO mode). 33 | pinMode(trigger_pin, OUTPUT); // Set trigger pin to output (on Teensy 3.x (ARM), pins default to disabled, at least one pinMode() is needed for GPIO mode). 34 | #endif 35 | 36 | #if defined (ARDUINO_AVR_YUN) 37 | pinMode(echo_pin, INPUT); // Set echo pin to input for the Arduino Yun, not sure why it doesn't default this way. 38 | #endif 39 | 40 | #if ONE_PIN_ENABLED != true && DO_BITWISE == true 41 | *_triggerMode |= _triggerBit; // Set trigger pin to output. 42 | #endif 43 | } 44 | 45 | 46 | // --------------------------------------------------------------------------- 47 | // Standard ping methods 48 | // --------------------------------------------------------------------------- 49 | 50 | unsigned int NewPing::ping(unsigned int max_cm_distance) { 51 | if (max_cm_distance > 0) set_max_distance(max_cm_distance); // Call function to set a new max sensor distance. 52 | 53 | if (!ping_trigger()) return NO_ECHO; // Trigger a ping, if it returns false, return NO_ECHO to the calling function. 54 | 55 | #if URM37_ENABLED == true 56 | #if DO_BITWISE == true 57 | while (!(*_echoInput & _echoBit)) // Wait for the ping echo. 58 | #else 59 | while (!digitalRead(_echoPin)) // Wait for the ping echo. 60 | #endif 61 | if (micros() > _max_time) return NO_ECHO; // Stop the loop and return NO_ECHO (false) if we're beyond the set maximum distance. 62 | #else 63 | #if DO_BITWISE == true 64 | while (*_echoInput & _echoBit) // Wait for the ping echo. 65 | #else 66 | while (digitalRead(_echoPin)) // Wait for the ping echo. 67 | #endif 68 | if (micros() > _max_time) return NO_ECHO; // Stop the loop and return NO_ECHO (false) if we're beyond the set maximum distance. 69 | #endif 70 | 71 | return (micros() - (_max_time - _maxEchoTime) - PING_OVERHEAD); // Calculate ping time, include overhead. 72 | } 73 | 74 | 75 | unsigned long NewPing::ping_cm(unsigned int max_cm_distance) { 76 | unsigned long echoTime = NewPing::ping(max_cm_distance); // Calls the ping method and returns with the ping echo distance in uS. 77 | #if ROUNDING_ENABLED == false 78 | return (echoTime / US_ROUNDTRIP_CM); // Call the ping method and returns the distance in centimeters (no rounding). 79 | #else 80 | return NewPingConvert(echoTime, US_ROUNDTRIP_CM); // Convert uS to centimeters. 81 | #endif 82 | } 83 | 84 | 85 | unsigned long NewPing::ping_in(unsigned int max_cm_distance) { 86 | unsigned long echoTime = NewPing::ping(max_cm_distance); // Calls the ping method and returns with the ping echo distance in uS. 87 | #if ROUNDING_ENABLED == false 88 | return (echoTime / US_ROUNDTRIP_IN); // Call the ping method and returns the distance in inches (no rounding). 89 | #else 90 | return NewPingConvert(echoTime, US_ROUNDTRIP_IN); // Convert uS to inches. 91 | #endif 92 | } 93 | 94 | 95 | unsigned long NewPing::ping_median(uint8_t it, unsigned int max_cm_distance) { 96 | unsigned int uS[it], last; 97 | uint8_t j, i = 0; 98 | unsigned long t; 99 | uS[0] = NO_ECHO; 100 | 101 | while (i < it) { 102 | t = micros(); // Start ping timestamp. 103 | last = ping(max_cm_distance); // Send ping. 104 | 105 | if (last != NO_ECHO) { // Ping in range, include as part of median. 106 | if (i > 0) { // Don't start sort till second ping. 107 | for (j = i; j > 0 && uS[j - 1] < last; j--) // Insertion sort loop. 108 | uS[j] = uS[j - 1]; // Shift ping array to correct position for sort insertion. 109 | } else j = 0; // First ping is sort starting point. 110 | uS[j] = last; // Add last ping to array in sorted position. 111 | i++; // Move to next ping. 112 | } else it--; // Ping out of range, skip and don't include as part of median. 113 | 114 | if (i < it && micros() - t < PING_MEDIAN_DELAY) 115 | delay((PING_MEDIAN_DELAY + t - micros()) / 1000); // Millisecond delay between pings. 116 | 117 | } 118 | return (uS[it >> 1]); // Return the ping distance median. 119 | } 120 | 121 | 122 | // --------------------------------------------------------------------------- 123 | // Standard and timer interrupt ping method support functions (not called directly) 124 | // --------------------------------------------------------------------------- 125 | 126 | boolean NewPing::ping_trigger() { 127 | #if DO_BITWISE == true 128 | #if ONE_PIN_ENABLED == true 129 | *_triggerMode |= _triggerBit; // Set trigger pin to output. 130 | #endif 131 | 132 | *_triggerOutput &= ~_triggerBit; // Set the trigger pin low, should already be low, but this will make sure it is. 133 | delayMicroseconds(4); // Wait for pin to go low. 134 | *_triggerOutput |= _triggerBit; // Set trigger pin high, this tells the sensor to send out a ping. 135 | delayMicroseconds(10); // Wait long enough for the sensor to realize the trigger pin is high. Sensor specs say to wait 10uS. 136 | *_triggerOutput &= ~_triggerBit; // Set trigger pin back to low. 137 | 138 | #if ONE_PIN_ENABLED == true 139 | *_triggerMode &= ~_triggerBit; // Set trigger pin to input (when using one Arduino pin, this is technically setting the echo pin to input as both are tied to the same Arduino pin). 140 | #endif 141 | 142 | #if URM37_ENABLED == true 143 | if (!(*_echoInput & _echoBit)) return false; // Previous ping hasn't finished, abort. 144 | _max_time = micros() + _maxEchoTime + MAX_SENSOR_DELAY; // Maximum time we'll wait for ping to start (most sensors are <450uS, the SRF06 can take up to 34,300uS!) 145 | while (*_echoInput & _echoBit) // Wait for ping to start. 146 | if (micros() > _max_time) return false; // Took too long to start, abort. 147 | #else 148 | if (*_echoInput & _echoBit) return false; // Previous ping hasn't finished, abort. 149 | _max_time = micros() + _maxEchoTime + MAX_SENSOR_DELAY; // Maximum time we'll wait for ping to start (most sensors are <450uS, the SRF06 can take up to 34,300uS!) 150 | while (!(*_echoInput & _echoBit)) // Wait for ping to start. 151 | if (micros() > _max_time) return false; // Took too long to start, abort. 152 | #endif 153 | #else 154 | #if ONE_PIN_ENABLED == true 155 | pinMode(_triggerPin, OUTPUT); // Set trigger pin to output. 156 | #endif 157 | 158 | digitalWrite(_triggerPin, LOW); // Set the trigger pin low, should already be low, but this will make sure it is. 159 | delayMicroseconds(4); // Wait for pin to go low. 160 | digitalWrite(_triggerPin, HIGH); // Set trigger pin high, this tells the sensor to send out a ping. 161 | delayMicroseconds(10); // Wait long enough for the sensor to realize the trigger pin is high. Sensor specs say to wait 10uS. 162 | digitalWrite(_triggerPin, LOW); // Set trigger pin back to low. 163 | 164 | #if ONE_PIN_ENABLED == true 165 | pinMode(_triggerPin, INPUT); // Set trigger pin to input (when using one Arduino pin, this is technically setting the echo pin to input as both are tied to the same Arduino pin). 166 | #endif 167 | 168 | #if URM37_ENABLED == true 169 | if (!digitalRead(_echoPin)) return false; // Previous ping hasn't finished, abort. 170 | _max_time = micros() + _maxEchoTime + MAX_SENSOR_DELAY; // Maximum time we'll wait for ping to start (most sensors are <450uS, the SRF06 can take up to 34,300uS!) 171 | while (digitalRead(_echoPin)) // Wait for ping to start. 172 | if (micros() > _max_time) return false; // Took too long to start, abort. 173 | #else 174 | if (digitalRead(_echoPin)) return false; // Previous ping hasn't finished, abort. 175 | _max_time = micros() + _maxEchoTime + MAX_SENSOR_DELAY; // Maximum time we'll wait for ping to start (most sensors are <450uS, the SRF06 can take up to 34,300uS!) 176 | while (!digitalRead(_echoPin)) // Wait for ping to start. 177 | if (micros() > _max_time) return false; // Took too long to start, abort. 178 | #endif 179 | #endif 180 | 181 | _max_time = micros() + _maxEchoTime; // Ping started, set the time-out. 182 | return true; // Ping started successfully. 183 | } 184 | 185 | 186 | void NewPing::set_max_distance(unsigned int max_cm_distance) { 187 | #if ROUNDING_ENABLED == false 188 | _maxEchoTime = min(max_cm_distance + 1, (unsigned int) MAX_SENSOR_DISTANCE + 1) * US_ROUNDTRIP_CM; // Calculate the maximum distance in uS (no rounding). 189 | #else 190 | _maxEchoTime = min(max_cm_distance, (unsigned int) MAX_SENSOR_DISTANCE) * US_ROUNDTRIP_CM + (US_ROUNDTRIP_CM / 2); // Calculate the maximum distance in uS. 191 | #endif 192 | } 193 | 194 | 195 | #if TIMER_ENABLED == true && DO_BITWISE == true 196 | 197 | // --------------------------------------------------------------------------- 198 | // Timer interrupt ping methods (won't work with non-AVR, ATmega128 and all ATtiny microcontrollers) 199 | // --------------------------------------------------------------------------- 200 | 201 | void NewPing::ping_timer(void (*userFunc)(void), unsigned int max_cm_distance) { 202 | if (max_cm_distance > 0) set_max_distance(max_cm_distance); // Call function to set a new max sensor distance. 203 | 204 | if (!ping_trigger()) return; // Trigger a ping, if it returns false, return without starting the echo timer. 205 | timer_us(ECHO_TIMER_FREQ, userFunc); // Set ping echo timer check every ECHO_TIMER_FREQ uS. 206 | } 207 | 208 | 209 | boolean NewPing::check_timer() { 210 | if (micros() > _max_time) { // Outside the time-out limit. 211 | timer_stop(); // Disable timer interrupt 212 | return false; // Cancel ping timer. 213 | } 214 | 215 | #if URM37_ENABLED == false 216 | if (!(*_echoInput & _echoBit)) { // Ping echo received. 217 | #else 218 | if (*_echoInput & _echoBit) { // Ping echo received. 219 | #endif 220 | timer_stop(); // Disable timer interrupt 221 | ping_result = (micros() - (_max_time - _maxEchoTime) - PING_TIMER_OVERHEAD); // Calculate ping time including overhead. 222 | return true; // Return ping echo true. 223 | } 224 | 225 | return false; // Return false because there's no ping echo yet. 226 | } 227 | 228 | 229 | // --------------------------------------------------------------------------- 230 | // Timer2/Timer4 interrupt methods (can be used for non-ultrasonic needs) 231 | // --------------------------------------------------------------------------- 232 | 233 | // Variables used for timer functions 234 | void (*intFunc)(); 235 | void (*intFunc2)(); 236 | unsigned long _ms_cnt_reset; 237 | volatile unsigned long _ms_cnt; 238 | #if defined(__arm__) && defined(TEENSYDUINO) 239 | IntervalTimer itimer; 240 | #endif 241 | 242 | 243 | void NewPing::timer_us(unsigned int frequency, void (*userFunc)(void)) { 244 | intFunc = userFunc; // User's function to call when there's a timer event. 245 | timer_setup(); // Configure the timer interrupt. 246 | 247 | #if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo). 248 | OCR4C = min((frequency>>2) - 1, 255); // Every count is 4uS, so divide by 4 (bitwise shift right 2) subtract one, then make sure we don't go over 255 limit. 249 | TIMSK4 = (1<>2) - 1, 255); // Every count is 4uS, so divide by 4 (bitwise shift right 2) subtract one, then make sure we don't go over 255 limit. 254 | TIMSK2 |= (1<= 100 135 | #include 136 | #else 137 | #include 138 | #include 139 | #endif 140 | 141 | #if defined (__AVR__) 142 | #include 143 | #include 144 | #endif 145 | 146 | // Shouldn't need to change these values unless you have a specific need to do so. 147 | #define MAX_SENSOR_DISTANCE 500 // Maximum sensor distance can be as high as 500cm, no reason to wait for ping longer than sound takes to travel this distance and back. Default=500 148 | #define US_ROUNDTRIP_CM 57 // Microseconds (uS) it takes sound to travel round-trip 1cm (2cm total), uses integer to save compiled code space. Default=57 149 | #define US_ROUNDTRIP_IN 146 // Microseconds (uS) it takes sound to travel round-trip 1 inch (2 inches total), uses integer to save compiled code space. Defalult=146 150 | #define ONE_PIN_ENABLED true // Set to "false" to disable one pin mode which saves around 14-26 bytes of binary size. Default=true 151 | #define ROUNDING_ENABLED false // Set to "true" to enable distance rounding which also adds 64 bytes to binary size. Default=false 152 | #define URM37_ENABLED false // Set to "true" to enable support for the URM37 sensor in PWM mode. Default=false 153 | #define TIMER_ENABLED true // Set to "false" to disable the timer ISR (if getting "__vector_7" compile errors set this to false). Default=true 154 | 155 | // Probably shouldn't change these values unless you really know what you're doing. 156 | #define NO_ECHO 0 // Value returned if there's no ping echo within the specified MAX_SENSOR_DISTANCE or max_cm_distance. Default=0 157 | #define MAX_SENSOR_DELAY 5800 // Maximum uS it takes for sensor to start the ping. Default=5800 158 | #define ECHO_TIMER_FREQ 24 // Frequency to check for a ping echo (every 24uS is about 0.4cm accuracy). Default=24 159 | #define PING_MEDIAN_DELAY 29000 // Microsecond delay between pings in the ping_median method. Default=29000 160 | #define PING_OVERHEAD 5 // Ping overhead in microseconds (uS). Default=5 161 | #define PING_TIMER_OVERHEAD 13 // Ping timer overhead in microseconds (uS). Default=13 162 | #if URM37_ENABLED == true 163 | #undef US_ROUNDTRIP_CM 164 | #undef US_ROUNDTRIP_IN 165 | #define US_ROUNDTRIP_CM 50 // Every 50uS PWM signal is low indicates 1cm distance. Default=50 166 | #define US_ROUNDTRIP_IN 127 // If 50uS is 1cm, 1 inch would be 127uS (50 x 2.54 = 127). Default=127 167 | #endif 168 | 169 | // Conversion from uS to distance (round result to nearest cm or inch). 170 | #define NewPingConvert(echoTime, conversionFactor) (max(((unsigned int)echoTime + conversionFactor / 2) / conversionFactor, (echoTime ? 1 : 0))) 171 | 172 | // Detect non-AVR microcontrollers (Teensy 3.x, Arduino DUE, etc.) and don't use port registers or timer interrupts as required. 173 | #if (defined (__arm__) && defined (TEENSYDUINO)) 174 | #undef PING_OVERHEAD 175 | #define PING_OVERHEAD 1 176 | #undef PING_TIMER_OVERHEAD 177 | #define PING_TIMER_OVERHEAD 1 178 | #define DO_BITWISE true 179 | #elif !defined (__AVR__) 180 | #undef PING_OVERHEAD 181 | #define PING_OVERHEAD 1 182 | #undef PING_TIMER_OVERHEAD 183 | #define PING_TIMER_OVERHEAD 1 184 | #undef TIMER_ENABLED 185 | #define TIMER_ENABLED false 186 | #define DO_BITWISE false 187 | #else 188 | #define DO_BITWISE true 189 | #endif 190 | 191 | // Disable the timer interrupts when using ATmega128 and all ATtiny microcontrollers. 192 | #if defined (__AVR_ATmega128__) || defined (__AVR_ATtiny24__) || defined (__AVR_ATtiny44__) || defined (__AVR_ATtiny84__) || defined (__AVR_ATtiny25__) || defined (__AVR_ATtiny45__) || defined (__AVR_ATtiny85__) || defined (__AVR_ATtiny261__) || defined (__AVR_ATtiny461__) || defined (__AVR_ATtiny861__) || defined (__AVR_ATtiny43U__) 193 | #undef TIMER_ENABLED 194 | #define TIMER_ENABLED false 195 | #endif 196 | 197 | // Define timers when using ATmega8, ATmega16, ATmega32 and ATmega8535 microcontrollers. 198 | #if defined (__AVR_ATmega8__) || defined (__AVR_ATmega16__) || defined (__AVR_ATmega32__) || defined (__AVR_ATmega8535__) 199 | #define OCR2A OCR2 200 | #define TIMSK2 TIMSK 201 | #define OCIE2A OCIE2 202 | #endif 203 | 204 | class NewPing { 205 | public: 206 | NewPing(uint8_t trigger_pin, uint8_t echo_pin, unsigned int max_cm_distance = MAX_SENSOR_DISTANCE); 207 | unsigned int ping(unsigned int max_cm_distance = 0); 208 | unsigned long ping_cm(unsigned int max_cm_distance = 0); 209 | unsigned long ping_in(unsigned int max_cm_distance = 0); 210 | unsigned long ping_median(uint8_t it = 5, unsigned int max_cm_distance = 0); 211 | static unsigned int convert_cm(unsigned int echoTime); 212 | static unsigned int convert_in(unsigned int echoTime); 213 | #if TIMER_ENABLED == true 214 | void ping_timer(void (*userFunc)(void), unsigned int max_cm_distance = 0); 215 | boolean check_timer(); 216 | unsigned long ping_result; 217 | static void timer_us(unsigned int frequency, void (*userFunc)(void)); 218 | static void timer_ms(unsigned long frequency, void (*userFunc)(void)); 219 | static void timer_stop(); 220 | #endif 221 | private: 222 | boolean ping_trigger(); 223 | void set_max_distance(unsigned int max_cm_distance); 224 | #if TIMER_ENABLED == true 225 | boolean ping_trigger_timer(unsigned int trigger_delay); 226 | boolean ping_wait_timer(); 227 | static void timer_setup(); 228 | static void timer_ms_cntdwn(); 229 | #endif 230 | #if DO_BITWISE == true 231 | uint8_t _triggerBit; 232 | uint8_t _echoBit; 233 | volatile uint8_t *_triggerOutput; 234 | volatile uint8_t *_echoInput; 235 | volatile uint8_t *_triggerMode; 236 | #else 237 | uint8_t _triggerPin; 238 | uint8_t _echoPin; 239 | #endif 240 | unsigned int _maxEchoTime; 241 | unsigned long _max_time; 242 | }; 243 | 244 | 245 | #endif 246 | -------------------------------------------------------------------------------- /libraries/Odometry/Odometry.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | //Global Functions 4 | void rightEncoderAChange(); 5 | void setupPinChangeInterrupt(byte pin); 6 | void leftEncoderAChange(); 7 | void leftEncoderBChange(); 8 | 9 | //Global Variables 10 | int rightEncoderCounter; 11 | int leftEncoderCounter; 12 | byte _rightEncoderAPin; 13 | byte _rightEncoderBPin; 14 | byte _leftEncoderAPin; 15 | byte _leftEncoderBPin; 16 | 17 | /** 18 | * Constructor arg are pins for channels A and B on right and left encoders 19 | **/ 20 | Odometry::Odometry(byte rightEncoderAPin, byte rightEncoderBPin, byte leftEncoderAPin, byte leftEncoderBPin, float wheelBase, float wheelDiameter, int cpr) { 21 | pinMode(rightEncoderAPin, INPUT); 22 | pinMode(rightEncoderBPin, INPUT); 23 | pinMode(leftEncoderAPin, INPUT); 24 | pinMode(leftEncoderBPin, INPUT); 25 | digitalWrite(leftEncoderBPin, HIGH); 26 | rightEncoderCounter = 0.; 27 | leftEncoderCounter = 0.; 28 | attachInterrupt(digitalPinToInterrupt(rightEncoderAPin), rightEncoderAChange, CHANGE); 29 | setupPinChangeInterrupt(rightEncoderBPin); 30 | attachInterrupt(digitalPinToInterrupt(leftEncoderAPin), leftEncoderAChange, CHANGE); 31 | attachInterrupt(digitalPinToInterrupt(leftEncoderBPin), leftEncoderBChange, CHANGE); 32 | _rightEncoderAPin = rightEncoderAPin; 33 | _rightEncoderBPin = rightEncoderBPin; 34 | _leftEncoderAPin = leftEncoderAPin; 35 | _leftEncoderBPin = leftEncoderBPin; 36 | _wheelBase = wheelBase; 37 | _wheelDiameter = wheelDiameter; 38 | _cpr = cpr; 39 | 40 | theta = 0; 41 | clock = millis(); 42 | } 43 | 44 | void Odometry::update() { 45 | //Calculate linear distance that each wheel has traveled 46 | float rightWheelDistance = ((float)rightEncoderCounter / _cpr) * _wheelDiameter * PI; 47 | float leftWheelDistance = ((float)leftEncoderCounter / _cpr) * _wheelDiameter * PI; 48 | 49 | //Calculate relative angle that robot has turned 50 | float dtheta = (rightWheelDistance - leftWheelDistance) / _wheelBase; 51 | //Calculate angular velocity 52 | vtheta = dtheta / (millis() - clock) * 1000; 53 | //Accumulate angles to calculate absolute heading 54 | theta += dtheta; 55 | 56 | //Decompose linear distance into its component values 57 | float meanWheelDistance = (rightWheelDistance + leftWheelDistance) / 2; 58 | x = meanWheelDistance * cos(dtheta); 59 | y = meanWheelDistance * sin(dtheta); 60 | //Calculate linear velocity 61 | vx = x / (millis() - clock) * 1000; 62 | vy = y / (millis() - clock) * 1000; 63 | 64 | //Reset counters 65 | rightEncoderCounter = 0; 66 | leftEncoderCounter = 0; 67 | 68 | //Reset clock 69 | clock = millis(); 70 | } 71 | 72 | void rightEncoderAChange() { 73 | bool rightEncoderAStatus = digitalRead(_rightEncoderAPin); 74 | bool rightEncoderBStatus = digitalRead(_rightEncoderBPin); 75 | if (((rightEncoderAStatus == HIGH) && (rightEncoderBStatus == LOW)) || ((rightEncoderAStatus == LOW) && (rightEncoderBStatus == HIGH))) { 76 | rightEncoderCounter++; 77 | } 78 | else { 79 | rightEncoderCounter--; 80 | } 81 | } 82 | 83 | ISR (PCINT0_vect) { 84 | bool rightEncoderAStatus = digitalRead(_rightEncoderAPin); 85 | bool rightEncoderBStatus = digitalRead(_rightEncoderBPin); 86 | if (((rightEncoderAStatus == HIGH) && (rightEncoderBStatus == HIGH)) || ((rightEncoderAStatus == LOW) && (rightEncoderBStatus == LOW))) { 87 | rightEncoderCounter++; 88 | } 89 | else { 90 | rightEncoderCounter--; 91 | } 92 | } 93 | 94 | void leftEncoderAChange() { 95 | bool leftEncoderAStatus = digitalRead(_leftEncoderAPin); 96 | bool leftEncoderBStatus = digitalRead(_leftEncoderBPin); 97 | if (((leftEncoderAStatus == HIGH) && (leftEncoderBStatus == HIGH)) || ((leftEncoderAStatus == LOW) && (leftEncoderBStatus == LOW))) { 98 | leftEncoderCounter++; 99 | } 100 | else { 101 | leftEncoderCounter--; 102 | } 103 | } 104 | 105 | void leftEncoderBChange() { 106 | bool leftEncoderAStatus = digitalRead(_leftEncoderAPin); 107 | bool leftEncoderBStatus = digitalRead(_leftEncoderBPin); 108 | if (((leftEncoderAStatus == HIGH) && (leftEncoderBStatus == LOW)) || ((leftEncoderAStatus == LOW) && (leftEncoderBStatus == HIGH))) { 109 | leftEncoderCounter++; 110 | } 111 | else { 112 | leftEncoderCounter--; 113 | } 114 | } 115 | 116 | void setupPinChangeInterrupt(byte pin) { 117 | *digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin)); // enable pin 118 | PCIFR |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt 119 | PCICR |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group 120 | } -------------------------------------------------------------------------------- /libraries/Odometry/Odometry.h: -------------------------------------------------------------------------------- 1 | #ifndef Odometry_h 2 | #define Odometry_h 3 | 4 | #include "Arduino.h" 5 | 6 | class Odometry { 7 | public: 8 | //Constructors 9 | Odometry(byte rightEncoderAPin, byte rightEncoderBPin, byte leftEncoderAPin, byte leftEncoderBPin, float wheelBase, float wheelDiameter, int cpr); 10 | 11 | //Functions 12 | void update(); 13 | 14 | //Variables 15 | float x, y, theta; 16 | float vx, vy, vtheta; 17 | long clock; 18 | 19 | private: 20 | //Variables 21 | float _wheelBase, _wheelDiameter; 22 | int _cpr; 23 | }; 24 | 25 | #endif -------------------------------------------------------------------------------- /libraries/Servo/Servo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 3 | Copyright (c) 2009 Michael Margolis. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #if defined(ARDUINO_ARCH_AVR) 21 | 22 | #include 23 | #include 24 | 25 | #include "Servo.h" 26 | 27 | #define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009 28 | #define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds 29 | 30 | 31 | #define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009 32 | 33 | //#define NBR_TIMERS (MAX_SERVOS / SERVOS_PER_TIMER) 34 | 35 | static servo_t servos[MAX_SERVOS]; // static array of servo structures 36 | static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval) 37 | 38 | uint8_t ServoCount = 0; // the total number of attached servos 39 | 40 | 41 | // convenience macros 42 | #define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo 43 | #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer 44 | #define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel 45 | #define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel 46 | 47 | #define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo 48 | #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo 49 | 50 | /************ static functions common to all instances ***********************/ 51 | 52 | static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA) 53 | { 54 | if( Channel[timer] < 0 ) 55 | *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer 56 | else{ 57 | if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true ) 58 | digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated 59 | } 60 | 61 | Channel[timer]++; // increment to the next channel 62 | if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { 63 | *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks; 64 | if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated 65 | digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high 66 | } 67 | else { 68 | // finished all channels so wait for the refresh period to expire before starting over 69 | if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed 70 | *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL); 71 | else 72 | *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed 73 | Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel 74 | } 75 | } 76 | 77 | #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform 78 | // Interrupt handlers for Arduino 79 | #if defined(_useTimer1) 80 | SIGNAL (TIMER1_COMPA_vect) 81 | { 82 | handle_interrupts(_timer1, &TCNT1, &OCR1A); 83 | } 84 | #endif 85 | 86 | #if defined(_useTimer3) 87 | SIGNAL (TIMER3_COMPA_vect) 88 | { 89 | handle_interrupts(_timer3, &TCNT3, &OCR3A); 90 | } 91 | #endif 92 | 93 | #if defined(_useTimer4) 94 | SIGNAL (TIMER4_COMPA_vect) 95 | { 96 | handle_interrupts(_timer4, &TCNT4, &OCR4A); 97 | } 98 | #endif 99 | 100 | #if defined(_useTimer5) 101 | SIGNAL (TIMER5_COMPA_vect) 102 | { 103 | handle_interrupts(_timer5, &TCNT5, &OCR5A); 104 | } 105 | #endif 106 | 107 | #elif defined WIRING 108 | // Interrupt handlers for Wiring 109 | #if defined(_useTimer1) 110 | void Timer1Service() 111 | { 112 | handle_interrupts(_timer1, &TCNT1, &OCR1A); 113 | } 114 | #endif 115 | #if defined(_useTimer3) 116 | void Timer3Service() 117 | { 118 | handle_interrupts(_timer3, &TCNT3, &OCR3A); 119 | } 120 | #endif 121 | #endif 122 | 123 | 124 | static void initISR(timer16_Sequence_t timer) 125 | { 126 | #if defined (_useTimer1) 127 | if(timer == _timer1) { 128 | TCCR1A = 0; // normal counting mode 129 | TCCR1B = _BV(CS11); // set prescaler of 8 130 | TCNT1 = 0; // clear the timer count 131 | #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__) 132 | TIFR |= _BV(OCF1A); // clear any pending interrupts; 133 | TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt 134 | #else 135 | // here if not ATmega8 or ATmega128 136 | TIFR1 |= _BV(OCF1A); // clear any pending interrupts; 137 | TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt 138 | #endif 139 | #if defined(WIRING) 140 | timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service); 141 | #endif 142 | } 143 | #endif 144 | 145 | #if defined (_useTimer3) 146 | if(timer == _timer3) { 147 | TCCR3A = 0; // normal counting mode 148 | TCCR3B = _BV(CS31); // set prescaler of 8 149 | TCNT3 = 0; // clear the timer count 150 | #if defined(__AVR_ATmega128__) 151 | TIFR |= _BV(OCF3A); // clear any pending interrupts; 152 | ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt 153 | #else 154 | TIFR3 = _BV(OCF3A); // clear any pending interrupts; 155 | TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt 156 | #endif 157 | #if defined(WIRING) 158 | timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only 159 | #endif 160 | } 161 | #endif 162 | 163 | #if defined (_useTimer4) 164 | if(timer == _timer4) { 165 | TCCR4A = 0; // normal counting mode 166 | TCCR4B = _BV(CS41); // set prescaler of 8 167 | TCNT4 = 0; // clear the timer count 168 | TIFR4 = _BV(OCF4A); // clear any pending interrupts; 169 | TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt 170 | } 171 | #endif 172 | 173 | #if defined (_useTimer5) 174 | if(timer == _timer5) { 175 | TCCR5A = 0; // normal counting mode 176 | TCCR5B = _BV(CS51); // set prescaler of 8 177 | TCNT5 = 0; // clear the timer count 178 | TIFR5 = _BV(OCF5A); // clear any pending interrupts; 179 | TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt 180 | } 181 | #endif 182 | } 183 | 184 | static void finISR(timer16_Sequence_t timer) 185 | { 186 | //disable use of the given timer 187 | #if defined WIRING // Wiring 188 | if(timer == _timer1) { 189 | #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) 190 | TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt 191 | #else 192 | TIMSK &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt 193 | #endif 194 | timerDetach(TIMER1OUTCOMPAREA_INT); 195 | } 196 | else if(timer == _timer3) { 197 | #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) 198 | TIMSK3 &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt 199 | #else 200 | ETIMSK &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt 201 | #endif 202 | timerDetach(TIMER3OUTCOMPAREA_INT); 203 | } 204 | #else 205 | //For arduino - in future: call here to a currently undefined function to reset the timer 206 | #endif 207 | } 208 | 209 | static boolean isTimerActive(timer16_Sequence_t timer) 210 | { 211 | // returns true if any servo is active on this timer 212 | for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) { 213 | if(SERVO(timer,channel).Pin.isActive == true) 214 | return true; 215 | } 216 | return false; 217 | } 218 | 219 | 220 | /****************** end of static functions ******************************/ 221 | 222 | Servo::Servo() 223 | { 224 | if( ServoCount < MAX_SERVOS) { 225 | this->servoIndex = ServoCount++; // assign a servo index to this instance 226 | servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009 227 | } 228 | else 229 | this->servoIndex = INVALID_SERVO ; // too many servos 230 | } 231 | 232 | uint8_t Servo::attach(int pin) 233 | { 234 | return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH); 235 | } 236 | 237 | uint8_t Servo::attach(int pin, int min, int max) 238 | { 239 | if(this->servoIndex < MAX_SERVOS ) { 240 | pinMode( pin, OUTPUT) ; // set servo pin to output 241 | servos[this->servoIndex].Pin.nbr = pin; 242 | // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 243 | this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS 244 | this->max = (MAX_PULSE_WIDTH - max)/4; 245 | // initialize the timer if it has not already been initialized 246 | timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); 247 | if(isTimerActive(timer) == false) 248 | initISR(timer); 249 | servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive 250 | } 251 | return this->servoIndex ; 252 | } 253 | 254 | void Servo::detach() 255 | { 256 | servos[this->servoIndex].Pin.isActive = false; 257 | timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); 258 | if(isTimerActive(timer) == false) { 259 | finISR(timer); 260 | } 261 | } 262 | 263 | void Servo::write(int value) 264 | { 265 | if(value < MIN_PULSE_WIDTH) 266 | { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds) 267 | if(value < 0) value = 0; 268 | if(value > 180) value = 180; 269 | value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX()); 270 | } 271 | this->writeMicroseconds(value); 272 | } 273 | 274 | void Servo::writeMicroseconds(int value) 275 | { 276 | // calculate and store the values for the given channel 277 | byte channel = this->servoIndex; 278 | if( (channel < MAX_SERVOS) ) // ensure channel is valid 279 | { 280 | if( value < SERVO_MIN() ) // ensure pulse width is valid 281 | value = SERVO_MIN(); 282 | else if( value > SERVO_MAX() ) 283 | value = SERVO_MAX(); 284 | 285 | value = value - TRIM_DURATION; 286 | value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009 287 | 288 | uint8_t oldSREG = SREG; 289 | cli(); 290 | servos[channel].ticks = value; 291 | SREG = oldSREG; 292 | } 293 | } 294 | 295 | int Servo::read() // return the value as degrees 296 | { 297 | return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180); 298 | } 299 | 300 | int Servo::readMicroseconds() 301 | { 302 | unsigned int pulsewidth; 303 | if( this->servoIndex != INVALID_SERVO ) 304 | pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION ; // 12 aug 2009 305 | else 306 | pulsewidth = 0; 307 | 308 | return pulsewidth; 309 | } 310 | 311 | bool Servo::attached() 312 | { 313 | return servos[this->servoIndex].Pin.isActive ; 314 | } 315 | 316 | #endif // ARDUINO_ARCH_AVR 317 | 318 | -------------------------------------------------------------------------------- /libraries/Servo/Servo.h: -------------------------------------------------------------------------------- 1 | /* 2 | Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 3 | Copyright (c) 2009 Michael Margolis. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | /* 21 | A servo is activated by creating an instance of the Servo class passing 22 | the desired pin to the attach() method. 23 | The servos are pulsed in the background using the value most recently 24 | written using the write() method. 25 | 26 | Note that analogWrite of PWM on pins associated with the timer are 27 | disabled when the first servo is attached. 28 | Timers are seized as needed in groups of 12 servos - 24 servos use two 29 | timers, 48 servos will use four. 30 | The sequence used to sieze timers is defined in timers.h 31 | 32 | The methods are: 33 | 34 | Servo - Class for manipulating servo motors connected to Arduino pins. 35 | 36 | attach(pin ) - Attaches a servo motor to an i/o pin. 37 | attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds 38 | default min is 544, max is 2400 39 | 40 | write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds) 41 | writeMicroseconds() - Sets the servo pulse width in microseconds 42 | read() - Gets the last written servo pulse width as an angle between 0 and 180. 43 | readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release) 44 | attached() - Returns true if there is a servo attached. 45 | detach() - Stops an attached servos from pulsing its i/o pin. 46 | */ 47 | 48 | #ifndef Servo_h 49 | #define Servo_h 50 | 51 | #include 52 | 53 | /* 54 | * Defines for 16 bit timers used with Servo library 55 | * 56 | * If _useTimerX is defined then TimerX is a 16 bit timer on the current board 57 | * timer16_Sequence_t enumerates the sequence that the timers should be allocated 58 | * _Nbr_16timers indicates how many 16 bit timers are available. 59 | */ 60 | 61 | // Architecture specific include 62 | #if defined(ARDUINO_ARCH_AVR) 63 | #include "ServoTimers.h" 64 | #else 65 | #error "This library only supports boards with an AVR processor." 66 | #endif 67 | 68 | #define Servo_VERSION 2 // software version of this library 69 | 70 | #define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo 71 | #define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo 72 | #define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached 73 | #define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds 74 | 75 | #define SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer 76 | #define MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER) 77 | 78 | #define INVALID_SERVO 255 // flag indicating an invalid servo index 79 | 80 | typedef struct { 81 | uint8_t nbr :6 ; // a pin number from 0 to 63 82 | uint8_t isActive :1 ; // true if this channel is enabled, pin not pulsed if false 83 | } ServoPin_t ; 84 | 85 | typedef struct { 86 | ServoPin_t Pin; 87 | volatile unsigned int ticks; 88 | } servo_t; 89 | 90 | class Servo 91 | { 92 | public: 93 | Servo(); 94 | uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure 95 | uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes. 96 | void detach(); 97 | void write(int value); // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds 98 | void writeMicroseconds(int value); // Write pulse width in microseconds 99 | int read(); // returns current pulse width as an angle between 0 and 180 degrees 100 | int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release) 101 | bool attached(); // return true if this servo is attached, otherwise false 102 | private: 103 | uint8_t servoIndex; // index into the channel data for this servo 104 | int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH 105 | int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH 106 | }; 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /libraries/Servo/ServoTimers.h: -------------------------------------------------------------------------------- 1 | /* 2 | Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 3 | Copyright (c) 2009 Michael Margolis. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | /* 21 | * Defines for 16 bit timers used with Servo library 22 | * 23 | * If _useTimerX is defined then TimerX is a 16 bit timer on the current board 24 | * timer16_Sequence_t enumerates the sequence that the timers should be allocated 25 | * _Nbr_16timers indicates how many 16 bit timers are available. 26 | */ 27 | 28 | /** 29 | * AVR Only definitions 30 | * -------------------- 31 | */ 32 | 33 | // Say which 16 bit timers can be used and in what order 34 | #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 35 | #define _useTimer5 36 | #define _useTimer1 37 | #define _useTimer3 38 | #define _useTimer4 39 | typedef enum { _timer5, _timer1, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t; 40 | 41 | #elif defined(__AVR_ATmega32U4__) 42 | #define _useTimer3 43 | typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t; 44 | 45 | #elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) 46 | #define _useTimer3 47 | #define _useTimer1 48 | typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t; 49 | 50 | #elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1281__) || defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega2561__) 51 | #define _useTimer3 52 | #define _useTimer1 53 | typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t; 54 | 55 | #else // everything else 56 | #define _useTimer1 57 | typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t; 58 | #endif 59 | 60 | -------------------------------------------------------------------------------- /libraries/Servo/library.properties: -------------------------------------------------------------------------------- 1 | name=Servo 2 | version=1.1.2 3 | author=Michael Margolis, Arduino 4 | maintainer=Arduino 5 | sentence=Allows Arduino/Genuino boards to control a variety of servo motors. 6 | paragraph=This library can control a great number of servos.
It makes careful use of timers: the library can control 12 servos using only 1 timer.
On the Arduino Due you can control up to 60 servos.
7 | category=Device Control 8 | url=http://www.arduino.cc/en/Reference/Servo 9 | architectures=avr -------------------------------------------------------------------------------- /readmeImages/ArduinoDebugDataInput.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoDebugDataInput.png -------------------------------------------------------------------------------- /readmeImages/ArduinoDebugDataOutput.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoDebugDataOutput.png -------------------------------------------------------------------------------- /readmeImages/ArduinoDebugFingersOpen.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoDebugFingersOpen.png -------------------------------------------------------------------------------- /readmeImages/ArduinoDebugHighCenter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoDebugHighCenter.png -------------------------------------------------------------------------------- /readmeImages/ArduinoDebugWristDown.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoDebugWristDown.png -------------------------------------------------------------------------------- /readmeImages/ArduinoIDEBoardType.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoIDEBoardType.png -------------------------------------------------------------------------------- /readmeImages/ArduinoIDELineEndingBaudRate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoIDELineEndingBaudRate.png -------------------------------------------------------------------------------- /readmeImages/ArduinoIDEOpenSketch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoIDEOpenSketch.png -------------------------------------------------------------------------------- /readmeImages/ArduinoIDEOpenSketch2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoIDEOpenSketch2.png -------------------------------------------------------------------------------- /readmeImages/ArduinoIDESerialMonitor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoIDESerialMonitor.png -------------------------------------------------------------------------------- /readmeImages/ArduinoIDESerialPort.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoIDESerialPort.png -------------------------------------------------------------------------------- /readmeImages/ArduinoIDESketchbookLocation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoIDESketchbookLocation.png -------------------------------------------------------------------------------- /readmeImages/ArduinoIDEUploadSketch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoIDEUploadSketch.png -------------------------------------------------------------------------------- /readmeImages/ArduinoIDEUploadSuccess.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/Swarmathon-Arduino/429cd81dd3686ba02a226bd3bc37d3f5d839b417/readmeImages/ArduinoIDEUploadSuccess.png --------------------------------------------------------------------------------