// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2013-05-08 - added seamless Fastwire support
// - added note about gyro calibration
// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
// 2012-06-20 - improved FIFO overflow handling and simplified read process
// 2012-06-19 - completely rearranged DMP initialization code and simplification
// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
// - add 3D math helper file to DMP6 example sketch
// - add Euler output and Yaw/Pitch/Roll output formats
// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
// 2012-05-30 - basic DMP initialization working
/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
// Motor and LED pins
#define MOT_B1_PIN 10
#define MOT_B2_PIN 11
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// state variables
static int state_ind = 0;
float prev_yaw = 1000;
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === HELPER FUNCTIONS ===
// ================================================================
// from https://courses.ideate.cmu.edu/16-223/f2018/text/lib/WheelDrive.html#wheeldrive-doxygen
void set_motor_pwm(int pwm)
{
if (pwm < 0) { // reverse speeds
analogWrite(MOT_B1_PIN, -pwm);
digitalWrite(MOT_B2_PIN, LOW);
} else { // stop or forward
digitalWrite(MOT_B1_PIN, LOW);
analogWrite(MOT_B2_PIN, pwm);
}
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// Initialize motor pins
pinMode(MOT_B1_PIN, OUTPUT);
pinMode(MOT_B2_PIN, OUTPUT);
digitalWrite(MOT_B1_PIN, LOW);
digitalWrite(MOT_B2_PIN, LOW);
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
// Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
// the baud timing being too misaligned with processor ticks. You must use
// 38400 or slower in these cases, or use some kind of external separate
// crystal solution for the UART timer.
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// wait for ready
// Serial.println(F("\nSend any character to begin DMP programming and demo: "));
// while (Serial.available() && Serial.read()); // empty buffer
// while (!Serial.available()); // wait for data
// while (Serial.available() && Serial.read()); // empty buffer again
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220); // 220 default
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
// other program behavior stuff here
// .
// .
// .
// if you are really paranoid you can frequently test in between other
// stuff to see if mpuInterrupt is true, and if so, "break;" from the
// while() loop to immediately process the MPU data
// .
// .
// .
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
float yaw = ypr[0];
Serial.print("yaw\t");
Serial.println(yaw);
if (prev_yaw == 1000) prev_yaw = yaw;
Serial.print("state:\t");
Serial.println(state_ind);
float yaw_diff = abs(yaw - prev_yaw)*(180/M_PI);
Serial.print("yaw_diff\t");
Serial.println(yaw_diff);
switch (state_ind){
case 0:
digitalWrite(MOT_B1_PIN, LOW);
digitalWrite(MOT_B2_PIN, LOW);
if (yaw_diff > 1) state_ind = 1;
break;
case 1:
digitalWrite(MOT_B1_PIN, LOW);
digitalWrite(MOT_B2_PIN, LOW);
if (yaw_diff < 1) state_ind = 2;
break;
case 2:
float p_const = 1;
float diff = 0 - yaw;
diff = atan2(sin(diff), cos(diff));
Serial.print("diff\t");
Serial.println(diff);
int std_diff = int(p_const * float(diff) * (255/(2*M_PI)));
Serial.print("std_diff\t");
Serial.println(std_diff);
set_motor_pwm(std_diff);
if (abs(std_diff) < 10 && yaw_diff < 1) state_ind = 0;
break;
}
prev_yaw = yaw;
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}
This sketch utilizes code from this exercise.
]]>The device created is a box that tricks users into trying to store something in it. While the user is relatively far away from the box, the lid will open to invite users to put something in it. However, when the user approaches the box, it closes the lid to prevent anything from being stored in it.
CODE:
#include
#define IR_PIN A0
Servo lid;
const int LID_PIN = 9;
// check if sensed distance is between specified threshold
// https://acroname.com/articles/linearizing-sharp-ranger-data
bool check_IR(int IR_PIN){
float vol = analogRead(IR_PIN)*(5.0/1024);
float dist = 2914/(vol + 5) - 1;
return (dist < 500);
}
void setup() {
lid.attach(LID_PIN);
lid.write(0);
}
void loop() {
if (check_IR(IR_PIN)) {
lid.write(60);
delay(100);
} else {
lid.write(0);
delay(100);
}
}
[\code]
VIDEO:
]]>
The Amazing Self-Righting Box is a device that automatically rights itself when tipped over. The deception is simple. The box indicates that the “right side” is one way, while the actual preferred orientation is another. This box exploits an inherent trust in labels. From nutritional labels to street signs, we are raised to trust the knowledge derived from our environment. But, not everything you read is true.
The orientation of the box is derived from an analog accelerometer. To reduce the noise from the servo motors, the Arduino is powered from a separate power supply. Additionally, a continuous software-based smoothing filter is applied to the data from the analog pins. A state machine based framework is used to control the logic of the box.
We used the design from Henry’s previous demo with a few modifications. Here are the original CAD file and modified bottom plate and new arm: demo5
/* Nicholas Paiva - 2018
* State machine and analog measurement template code
*/
/******************** DEPENDENCIES ********************/
#include <Servo.h>
/******************** FORWARD DECLARATIONS ********************/
#define SECOND 1000000
#define MINUTE 60 * SECOND
//Utility
bool wait(uint32_t delay_time);
int16_t get_analog(uint8_t pin);
//States
void transition_state(uint8_t state_num);
void attach_state(uint8_t state_num, void (*function)(void));
bool interrupt();
//Flags
void trigger_flag(uint8_t flag);
void reset_flag(uint8_t flag);
void set_flag(uint8_t flag, bool value);
bool get_flag(uint8_t flag);
void dig_read_and_set(uint8_t flag, uint8_t pin);
/******************** USER DEFINED SECTION ********************/
/* FLAG IDS */
#define TIP_FLAG 0
#define NUM_FLAGS 1
/* STATE IDS */
#define IDLE_STATE 0
#define TIP_STATE 1
#define NUM_STATES 2
/* MOVING AVERAGE PARAMETERS */
#define NUM_PINS 3
#define NUM_SAMPLES 8
/* PIN DEFINITIONS */
#define X_PIN 0
#define Y_PIN 1
#define Z_PIN 2
#define ARM_PIN 9
/* MISC CONSTANTS */
#define TIP_THRESHOLD 375
Servo arm_servo;
/* user_setup:
* Put any setup code you need to add to this framework here.
* This function will run before the main loop starts.
*/
void user_setup(){
Serial.begin(9600);
arm_servo.attach(ARM_PIN);
arm_servo.write(90);
wait(SECOND);
reset_flag(TIP_FLAG);
}
/* interrupt:
* **ADVANCED**
* This function is used for any code that must happen as immediately
* as possible. Not a true hardware interrupt, but is called during
* delays or between states. This function can be used to interrupt
* the normal state flow when specific events occur, or execute
* user-defined background tasks. Use with caution, because this has
* the potential to make your state machine harder to understand.
*
* If this function returns true when called, the machine will stop
* whatever it is doing and transition to the selected state.
*/
bool interrupt(){
/*if(state == IDLE_STATE && false){
transition_state(RUN1_STATE);
return true;
}*/
return false;
}
/* set_flags:
* This function is called every time the main loop runs, before the
* current state handler function is called. Use this function to
* check on any pins you need to keep track of and set flags for later.
*
* For example, you can use "dig_read_and_set" to read a digital pin and
* store its value in a flag, and then check that flag later in your
* state machine.
*/
void set_flags(){
/* USER DEFINED CODE HERE*/
if(get_analog(Y_PIN) > TIP_THRESHOLD) trigger_flag(TIP_FLAG);
}
/* PUT YOUR STATE MACHINE HANDLERS HERE */
void idle_handler(){
arm_servo.write(90);
if(get_flag(TIP_FLAG)){
transition_state(TIP_STATE);
}
}
void tip_handler(){
//Serial.println("1");
wait(random(2*SECOND, SECOND*6));
for(uint8_t i = 0; i < 20; i++){
arm_servo.write(70-i);
wait(50000);
arm_servo.write(50+i);
wait(50000);
}
arm_servo.write(0);
wait(SECOND);
reset_flag(TIP_FLAG);
transition_state(IDLE_STATE);
}
/* END HANDLER SECTION */
/* Helper function for defining our state machine.
* Use as follows:
* 1. Create a handler function for each state of the type void fn(void)
* 2. Count the number of states, n
* 3. Create a unique ID for each state in the range [0, n)
* 4. Call attach_state(ID, &fn) to define each state
*/
void define_states(){
attach_state(IDLE_STATE, &idle_handler);
attach_state(TIP_STATE, &tip_handler);
}
/******************** MOVING AVERAGE LIBRARY ********************/
int16_t avg_samples [NUM_PINS][NUM_SAMPLES];
uint8_t ind [NUM_PINS];
int16_t avg_total [NUM_PINS];
int16_t avg [NUM_PINS];
/* Update one specific rolling average */
void update_avg(uint8_t pin){
uint8_t i = ind[pin];
//Subtract old value
avg_total[pin] = avg_total[pin] - avg_samples[pin][i];
//Measure and add new value
avg_samples[pin][i] = analogRead(pin);
avg_total[pin] = avg_total[pin] + avg_samples[pin][i];
//Increment and wrap counter
ind[pin] = i + 1;
ind[pin] %= (uint8_t)NUM_SAMPLES;
//Calculate the average
avg[pin] = avg_total[pin] / NUM_SAMPLES;
}
/* Update all of the rolling averages */
void update_avgs(){
for(uint8_t i = 0; i < NUM_PINS; i++){
update_avg(i);
}
}
/* Initializes the data to 0 for all of the rolling average code */
void init_avgs(){
//Zero out AVG array
for(uint8_t i = 0; i < NUM_PINS; i++){
for(uint8_t j = 0; j < NUM_SAMPLES; j++){
avg_samples[i][j] = 0;
}
ind[i] = 0;
avg_total[i] = 0;
avg[i] = 0;
}
}
/* Helper function for getting average value with error checking */
int16_t get_analog(uint8_t pin){
if(pin < NUM_PINS) //If a safe access, return the average
return avg[pin];
else //Otherwise return an error
return -1;
}
/******************** THRESHOLD / SWITCH CHECKING LIBRARY ********************/
bool flags[NUM_FLAGS] = {0};
/* Helper function for setting a particular flag, with error checking */
void set_flag(uint8_t flag, bool value){
if(flag < NUM_FLAGS) //If outside of our array, do nothing
flags[flag] = value;
}
/*Helper function to set a flag to 1 regardless of anything else */
void trigger_flag(uint8_t flag){
set_flag(flag, 1);
}
/*Helper function to set a flag to 0 regardless of anything else */
void reset_flag(uint8_t flag){
set_flag(flag, 0);
}
/* Helper function to set a flag to the digital read value of a pin */
void dig_read_and_set(uint8_t flag, uint8_t pin){
set_flag(flag, digitalRead(pin));
}
/* Helper function for getting a flag value */
bool get_flag(uint8_t flag){
if(flag < NUM_FLAGS)
return flags[flag];
else
return false;
}
/******************** STATE MACHINE LIBRARY ********************/
/* Current state */
uint8_t state = 0;
/* Array for state handling functions */
void (*handler_array[NUM_STATES])(void) = {NULL};
/* Helper function for setting up our state machine */
void attach_state(uint8_t state_num, void (*function)(void)){
handler_array[state_num] = function;
}
/* Helper function for changing the state */
void transition_state(uint8_t state_num){
state = state_num;
}
/******************** ARDUINO BOILERPLATE ********************/
void setup() {
init_avgs(); //Initialize our analog reading code
define_states(); //Setup our state machine
user_setup(); //Give the user opportunities to setup things
}
void do_background_tasks(){
//Do our analog measurements
update_avgs();
//Do our pin checks, set our flags
set_flags();
/*
Serial.print(get_analog(X_PIN));
Serial.print(",");
Serial.print(get_analog(Y_PIN));
Serial.print(",");
Serial.print(get_analog(Z_PIN));
Serial.println(",");
*/
if(state == TIP_STATE){
tone(5, get_analog(Y_PIN) - get_analog(X_PIN) + 300);
} else {
noTone(5);
}
}
void loop() {
//Do the stuff that goes on in the background
do_background_tasks();
//Allow for interrupts to the current state machine flow
interrupt();
//Call the current state handler
handler_array[state]();
}
/* wait:
* Special delay function that delays our current state machine code,
* but still executes background tasks. No state transition may occur
* during this time. Analog measurements are updated, and flags are
* set.
*/
bool wait(uint32_t delay_time){
//Get the current time and setup a tracker variable
uint32_t last_update = micros();
uint32_t d_t = 0;
//If we have not waited enough time yet, keep doing background tasks until we have
while(d_t < delay_time){
uint32_t now = micros();
//Add the time elapsed to our tracker variable
d_t += (now - last_update);
last_update = now;
//Do our background tasks while we wait
do_background_tasks();
//Allow for interrupts to the current state machine flow
if(interrupt()) return true;
}
return false;
}
]]>This prototype machine allows you to play fetch with your dog hands-free under a sort-of fixed-ratio reinforcement schedule. The machine will most dominantly fake throw a ping pong ball. It will then actually catapult the ball after a fixed number of times of fake throws. A distance sensor is used as part of determining machine states. Being close means that the machine will act playful and ready to throw without actually giving up possession of the ball. Once at a desirable distance away, the machine will decide to either throw or fake-out the user.
Disclaimer: repeated use this machine may cause your dog to become highly skeptical when trying to play fetch.
</pre>
#include <Servo.h>
Servo hit;
Servo throw_b;
const int big_angle1=0;
const int big_angle2=80;
const int small_angle1=170;
const int small_angle2=100;
const int echoPin=11;
const int trigPin=12;
const int SMALL_FAKE=0;
const int BIG_FAKE=1;
const int THRESHOLD=70;
float total_dis,avg_dis;
int mode=0;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
hit.attach(9);
throw_b.attach(10);
hit.write(small_angle1);
throw_b.write(big_angle1);
}
void loop() {
int count=0;
total_dis=0;
//averaging multiple readings to filter out errors
while(count<100)
{
total_dis+=detect_distance();
count+=1;
}
avg_dis=total_dis/(count+1);
if(avg_dis<=THRESHOLD)
{
mode=SMALL_FAKE;
}
else mode=BIG_FAKE;
if(mode==BIG_FAKE)
{
// put your main code here, to run repeatedly:
hit.write(small_angle2);
delay(200);
hit.write(small_angle1);
delay(500);
throw_b.write(big_angle2);
delay(500);
throw_b.write(big_angle1);
delay(5000);
Serial.println(mode);
}
else if(mode==SMALL_FAKE)
{
throw_b.write(20);
delay(10);
throw_b.write(0);
delay(10);
throw_b.write(20);
delay(10);
throw_b.write(0);
delay(100);
}
}
float detect_distance()
{
float duration, inches, cm;
pinMode(trigPin, OUTPUT);
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);
cm = microsecondsToCentimeters(duration);
return cm;
}
float microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}
]]>The robot is designed to respond negatively when someone gets close to it. using the ultrasonic sensor, it detects whether an object is too close to it. When the approaching object gets to close, the robot panics and attempts to scare the approaching threat by shaking its arms vigorously. When the object retreats, the robot calms down and returns to its initial state.
The robot is constructed using 6mm plywood, 2 servo motors (one for each arm) and an ultrasonic sensor.
CAD:Demo4 (1)
VIDEO:
CODE:
#include <NewPing.h>
#include <Servo.h>
const int ARM1_PIN = 9;
const int ARM2_PIN = 10;
Servo arm1;
Servo arm2;
int armPos1 = 12;
int armPos2 = 12;
static int state = 0;
static long elapsed_time = 0;
static long lastPingTime = 0;
//static int avgAc = 10;
static int distanceVals[10];
static int updateCount = 0;
static int distance = -1; // start at -1 to avoid triggering any behaviors before setup is complete. Check 0< when doing cases
static unsigned long last_update_clock = 0; // from class slides to set up timer
#define TRIGGER_PIN 12 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 11 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
void setup() {
Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.
arm1.attach(ARM1_PIN);
arm2.attach(ARM2_PIN);
}
//void moveHead(int directionMotion, int location, int timeMove)
//{
//}
long takeAverage(int a[], int len)
{
int sum = 0;
for (int i = 0; i < len; i++)
{
sum += a[i];
}
return sum / (double)len;
}
int moveServo(Servo servo, int lastPos, int nextPos, int duration)
{
unsigned long last_update_clock = micros();
int currentLocation = lastPos;
long magnitude = (abs(nextPos - lastPos));
if(magnitude < 5){
return 0;
}
long timeSteps = duration / magnitude/1000;
int servoSteps = magnitude / (duration/1000 / timeSteps);
int loops = 1;
int runningTotal = lastPos;
while (duration > 0) {
unsigned long now = micros();
unsigned long interval = now - last_update_clock;
last_update_clock = now;
duration -= interval;
if (lastPos > nextPos)
{
servo.write(lastPos + (loops * servoSteps));
}
else
{
servo.write(lastPos - (loops * servoSteps));
}
loops += 1;
updateDistance(interval);
if (updateState() != 0)
{
return 1;
}
}
return 0;
}
int updateState()
{
if (distance > 70 || distance == 0) //account of for out of range
{
if (state != 0)
{
state = 0;
return 1;
}
}
if (distance < 40 && distance >0)
{
if (state != 1)
{
state = 1;
return 1;
}
}
return 0;
}
void updateDistance(long delays)
{
if (lastPingTime <= 0)
{
if (updateCount = 10)
{
distance = takeAverage(distanceVals, 10); //change to take an average of a few measurements
updateCount = 0;
}
distanceVals[updateCount] = int(sonar.ping_cm());
updateCount += 1;
distance = sonar.ping_cm();
lastPingTime = 1000; // set delay here
}
else
{
lastPingTime -= delays;
}
}
void loop() {
unsigned long now = micros();
unsigned long interval = now - last_update_clock;
last_update_clock = now;
updateDistance(interval);
updateState();
switch (state)
{
case 0:
if (moveServo(arm1, armPos1, 20, 1000)) {
break;
}
break;
case 1:
if (moveServo(arm1, armPos1, 180, 2000)) {
break;
}
}
}
]]>
The mechanism uses a photoresistor for measuring light, and a servo that manipulates the spring. The model responds to whatever entity provides it with a sufficient light source. That could be the sun, or simply someone nearby with a flashlight in-hand.
<h3>Video</h3>
<h3>Files</h3>
<a href=”https://courses.ideate.cmu.edu/16-223/f2018/work/wp-content/uploads/2018/09/solidworks.zip”>solidworks</a>
<h3>Code</h3>
#include <Servo.h>
// CONSTANTS
const int SERVO_PIN = 9;
const int BUTTON_PIN = 2;
const int PHOTORESISTOR_PIN = A0;
const int MIN_SERVO_ANGLE = 10;
const int DEFAULT_SERVO_ANGLE = 90;
const int MAX_SERVO_ANGLE = 170;
const int ACCEPTABLE_LIGHT = 800;
// globals
Servo servo;
float servo_angle = DEFAULT_SERVO_ANGLE;
float smooth_servo_speed = 0.005;
float jittery_servo_speed = 0.008;
int servo_direction = 1;
int fixated_angle = DEFAULT_SERVO_ANGLE;
int photoresistor_value = 0;
bool personality = true; // false => smooth, true => jittery
bool state = false; // false => search, true => fixated
void setup() {
Serial.begin(9600);
pinMode(BUTTON_PIN, INPUT);
servo.attach(SERVO_PIN);
}
void set_new_servo_angle(float servo_speed, int minimum, int maximum, int multiplier) {
servo_angle = servo_angle + (servo_speed * servo_direction * multiplier);
if (servo_angle > maximum) {
servo_angle = maximum;
servo_direction = -1;
}
if (servo_angle < minimum) { servo_angle = minimum; servo_direction = 1; } } bool smooth_search() { if (photoresistor_value >= ACCEPTABLE_LIGHT) {
fixated_angle = servo_angle;
return true; // go to fixated state
}
set_new_servo_angle(smooth_servo_speed, MIN_SERVO_ANGLE, MAX_SERVO_ANGLE, 1);
return false;
}
bool smooth_fixated() {
if (photoresistor_value < ACCEPTABLE_LIGHT) { return false; // go back to searching state } set_new_servo_angle(smooth_servo_speed, fixated_angle - 20, fixated_angle + 20, 1); return true; } int get_rand_direction(int sample) { if (random(0, sample) == 0) { return -1; } return 1; } bool jittery_search() { if (photoresistor_value >= ACCEPTABLE_LIGHT) {
fixated_angle = servo_angle;
return true; // go to fixated state
}
int jumpyness = 1;
if (get_rand_direction(3000) == -1) {
jumpyness = 6000;
}
set_new_servo_angle(jittery_servo_speed, MIN_SERVO_ANGLE, MAX_SERVO_ANGLE, jumpyness);
servo_direction = servo_direction * get_rand_direction(2000);
return false;
}
bool jittery_fixated() {
if (photoresistor_value < ACCEPTABLE_LIGHT) {
return false; // go back to searching state
}
int jumpyness = 1;
if (get_rand_direction(10000) == -1) {
jumpyness = 80000;
set_new_servo_angle(jittery_servo_speed, fixated_angle - 20, fixated_angle + 20, jumpyness);
servo.write(random(MIN_SERVO_ANGLE, MAX_SERVO_ANGLE));
delay(1000);
}
set_new_servo_angle(jittery_servo_speed, fixated_angle - 20, fixated_angle + 20, jumpyness);
return true;
}
void loop() {
servo.write(servo_angle);
photoresistor_value = analogRead(PHOTORESISTOR_PIN);
if (personality == false) {
if (state == false) {
state = smooth_search();
} else {
state = smooth_fixated();
}
} else {
if (state == false) {
state = jittery_search();
} else {
state = jittery_fixated();
}
}
}
]]>
Henry Zhang, Vivek Anand
(Partner – Henry Zhang)
The mechanism built consists of two robots that push switches to interact with each other. Each robot consists of a table with a pushrod attached to a wheel driven by a servo motor. When one of the buttons are pushed, the motion is initiated. One robot pushes on its respective switch, which tells the second robot to push its switch. This causes the first robot to disengage the switch and soon afterwards the second one follows suit. the first robot receives the signal that the second robot’s switch is not pressed, and so it pushes its switch. This cycle continues until the Arduino board is switched off.
The mechanism is created using 6mm laser cut plywood. A revision to the design would be to make sturdier connections between the panels and a ramp to support the pushrod when disengaged. These are the reasons as to why much tape was used. Additionally, due to the weak interlocking system for panels, the switches could not be bolted down as the force from the pushrod would collapse the support for the switches.
VIDEO:
CODE:
#include <Servo.h>
const int SERVOA_PIN = 10;
const int SERVOB_PIN = 11;
Servo servoA;
Servo servoB;
bool servoA_run;
bool servoB_run;
void setup() {
// put your setup code here, to run once:
pinMode(8, INPUT); // Switch A
pinMode(9, INPUT); // Switch B
servoA.attach(SERVOA_PIN);
servoB.attach(SERVOB_PIN);
// initialize servo_run to false
servoA_run = false;
servoB_run = false;
// initialize servo postion
servoA.write(10);
servoB.write(30);
// debug serial stream
Serial.begin(9600);
}
void servoA_push() {
servoA.write(90);
delay(500);
}
void servoA_retrieve() {
servoA.write(10);
delay(500);
}
void servoB_push() {
servoB.write(130);
delay(500);
}
void servoB_retrieve() {
servoB.write(30);
delay(500);
}
void move_servo() {
if (servoA_run) {
servoA_push();
} else {
servoA_retrieve();
}
if (servoB_run) {
servoB_push();
} else {
servoB_retrieve();
}
}
void loop() {
// put your main code here, to run repeatedly:
int switchValA = digitalRead(8);
int switchValB = digitalRead(9);
if (switchValA == 1) {
// Servo2 start
Serial.println(“servo B start”);
servoB_run = true;
} else {
servoB_run = false;
}
if (switchValB == 1) {
// Servo1 start
Serial.println(“servo A start”);
servoA_run = true;
} else {
servoA_run = false;
}
move_servo();
delay(500);
}
NOTEPAD LINK TO FORMATTED CODE:
SOLIDWORKS PARTS:
]]>These responsive cars are made to communicate through sensing the world through limit switches. One car moves only when its limit switch is open, while the other moves only when its limit switch is closed. They are intended to be placed in front of each other so that they seem to chase one another around.
One of the subgoals for this demo was having simplicity of design. Both cars use just two laser-cut pieces of 6mm plywood, a DC motor, and a limit switch. The tradeoff to this is that the cars tend to turn in the rightward direction instead of driving straight. Wires tethered to a computer and wall outlet just amplify the issue further.
const int MOTOR_PIN_1 = 5;
const int MOTOR_PIN_2 = 6;
const int LIMIT_SWITCH_PIN = 13;
void setup() {
// put your setup code here, to run once:
pinMode(MOTOR_PIN_1, OUTPUT);
pinMode(MOTOR_PIN_2, OUTPUT);
pinMode(LIMIT_SWITCH_PIN, INPUT);
}
// https://courses.ideate.cmu.edu/16-223/f2018/text/lib/WheelDrive.html
void set_motor_pwm(int pwm, int MOTOR_PIN_1, int MOTOR_PIN_2)
{
if (pwm < 0) { // reverse speeds
analogWrite(MOTOR_PIN_1, -pwm);
digitalWrite(MOTOR_PIN_2, LOW);
} else { // stop or forward
digitalWrite(MOTOR_PIN_1, LOW);
analogWrite(MOTOR_PIN_2, pwm);
}
}
// https://courses.ideate.cmu.edu/16-223/f2018/text/lib/WheelDrive.html
void set_motor_currents(int pwm_A, int pwm_B)
{
set_motor_pwm(pwm_A, MOTOR_PIN_1, MOTOR_PIN_2);
set_motor_pwm(pwm_B, MOTOR_PIN_1, MOTOR_PIN_2);
}
// https://courses.ideate.cmu.edu/16-223/f2018/text/lib/WheelDrive.html
void spin_and_wait(int pwm_A, int pwm_B, int duration)
{
set_motor_currents(pwm_A, pwm_B);
delay(duration);
}
void loop() {
int value = digitalRead(LIMIT_SWITCH_PIN);
if (value == 0) {
spin_and_wait(255,255,2000);
} else {
spin_and_wait(0,0,50);
}
}
]]>
When you plug this little robot in, all he wants to do is unplug himself and go back to sleep. Unfortunately for him, the cable is so far away and he only has a stubby little servo arm. So, he has to strategically boost a nearby pendulum (like pushing a swing) until it knocks over the release lever which unplugs his power cable.
To achieve this dynamical boosting, the robot must keep track of where the pendulum is in order to push it at just the right time. Unfortunately, this little guy doesn’t have any sensors; so, he must rely on an in-depth knowledge of dynamics to estimate where the pendulum is at any given point in time based on where, when, and for how long he pushed it based on the following principles:
Pratfall Dynamics
Unfortunately for the robot, his pushes were just not strong enough to overcome the frictional losses in the system; so, the pendulum couldn’t maintain any momentum after he let go; so, he needed a bit of a helping hand to get unplugged.
#include <Servo.h>
#define P_SERV 11 // MOSI on ICSP header
Servo S1;
#define sgnf(x) ( (x) ? (x) / abs(x) : 0) // Basic sign function
// Constants (SI units):
#define MU_A 18e-6f // [Pa s] Dynamic Viscosity of Air at 20C, 1atm
#define grav 9.0867f // [m s-2] Standard Gravitational Acceleration
#define deg2rad M_PI / 180.0f
#define rad2deg 180.0f / M_PI
// Mechanical Properties (SI units):
#define M 0.04275f // [kg] Mass of Pendulum Arm
#define L 0.12460f // [m] Total Length of Pendulum Arm
#define LCM 0.06387f // [m] Distance to Center of Mass of Pendulum Arm from Center of Rotation
#define TSM 0.1765f // [N-m] Standard Torque Output of Servo Motor at 5V.
#define LH 0.02f // [m] Distance to Point where Servo Horn Contacts the Pendulum from the Servo's Center of Rotation
// Settings:
#define TH_PUSH (1.3f * deg2rad) // [rad] Magnitude of Angle of Pendulum at Which Servo should begin a Push.
#define TH_INIT (8.97f * deg2rad) // [rad] Angle at which Pendulum Starts.
#define THS_INIT 80 // [deg] Angle at which Servo Starts.
#define THS_REST 90 // [deg] Angular Distance of Resting Spot of Servo Horn Relative from 90 degrees (vertical) - just outside of the swing of the pendulum arm.
#define KD 24.0f // Tunable Linear Drag Coefficient
#define KF 0.0f // Tunable Constant Acceleration Losses due to Frictional Moments in Pendulum Bearing
// Pre-Computed Properties:
float inv_I = 1.0f / M / LCM / LCM; // Inverse Moment of Inertia of Pendulum Arm
float alpha_app = TSM * L * inv_I / LH; // Angular Acceleration Experience By Level
float k_drag = KD * MU_A * L*L*L * inv_I / 6.0f; // Contant Parameters in Drag Calculation
float k_acc = grav / LCM; // Contant Parameters in Standard Acceleration
// State Variables:
int pushing = 0; // Whether the Servo is Currently Pushing the Pendulum
unsigned long last_model_update = 0; // Millis Time of Previous Model Update
float alpha = 0; // [rad s-2] Current Estimated Angular Acceleration of Pendulum
float omega = 0; // [rad s-1] Current Estimated Angular Velocity of Pendulum
float theta = 0; // [rad] Current Estimated Angle of the Pendulum
/****
* Perform an update on the model of the pendulum factoring in whether or not the servo is currently pushing the pendulum (and in what direction).
* pushing = 1 -> pushing pendulum CCW
* pushing = -1 -> pushing pendulum CW
* pushing = 0 -> not pushing pendulum
*
* Returns: Current Estimated Position of Pendulum Arm
**/
void updateModel(){
// Basic Mid-Point Algorithm
unsigned long t = millis();
float dt = (t - last_model_update) / 1000.0f;
last_model_update = t; // Set immediately
omega += alpha * dt / 2;
theta += omega * dt / 2;
alpha = - k_acc * sinf(theta);
- k_drag * omega
- KF * sgnf(omega)
+ pushing ? pushing * alpha_app : 0;
omega += alpha * dt / 2;
theta += omega * dt / 2;
}
void setup(){
Serial.begin(115200);
S1.attach(P_SERV);
// Get to Staring Position and Sit:
S1.write(90 - THS_INIT);
delay(1000);
// Release Pendulum and Set Initial Conditions for Model
S1.write(90 - THS_REST);
theta = TH_INIT;
omega = 0.0f; // Starts from Rest
last_model_update = millis();
} // #setup
void loop(){
static unsigned long last_model_printout = 0; // Time of Previous Serial Model Update
// Check Model to Determine if Push is Needed (/before/ updating model):
if(abs(theta) < TH_PUSH){ pushing = -sgnf(theta); // Push by running servo to other resting position as fast as possible S1.write(90 + sgnf(theta)*THS_REST); } if(pushing && abs(theta) > TH_INIT){
pushing = 0;
}
updateModel();
if(millis() - last_model_printout > 50){ // Plot points infrequently so as to not continuously disturb model.
Serial.print(10*pushing);Serial.print(","); Serial.println(theta * rad2deg);
last_model_printout = millis();
}
delay(2);
} // #loop
Ele-thump is a lasercut pratfall device which combines the principles of stored spring energy and changing its mass distribution in order to cause itself to fall over at the user’s command. It’s name is a due to the fact that the released arm resembles an elephant’s trunk and it makes a thumping sound upon release.
The design’s key features are the living hinges that both to allow for the storing of spring energy while also proving an aesthetically pleasing design.
Ele-thump begins its life with the servo holding the “trunk” above its head to keep the center of mass over the platform and prevent tipping. By doing so, the spring force of the living hinges is primed to be released and explosively shift the center of mass when the servo rotates and allows the “trunk” to shoot forward. With the simple push of a button Ele-thump preforms a pratfall and completes its purpose in life.
#include <Servo.h>
const int SERVO_PIN = 9;
Servo motion;
const int BUTTON_PIN = 8;
int initLocation = 0;
void setup() {
motion.attach(SERVO_PIN);
motion.write(initLocation);
pinMode(BUTTON_PIN, INPUT);
}
void loop() {
int isPushed;
isPushed = digitalRead(BUTTON_PIN);
if(isPushed == HIGH)
{
motion.write(170);
delay(1000);
}
else{
motion.write(initLocation);
}
}
]]>