Revision 0:eae9712515a6, committed 2012-12-15

Comitter:

theschrade54

Date:

Sat Dec 15 01:48:19 2012 +0000

Commit message:

Semi-Functional EtchSketch Smartbot code. Takes x/y values and draws with polar coords.

Changed in this revision

diff -r 000000000000 -r eae9712515a6 HMC6352.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Sat Dec 15 01:48:19 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/HMC6352/#83c0cb554099

diff -r 000000000000 -r eae9712515a6 Motordriver.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motordriver.lib	Sat Dec 15 01:48:19 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/littlexc/code/Motordriver/#3110b9209d3c

diff -r 000000000000 -r eae9712515a6 Servo/Servo.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo/Servo.cpp	Sat Dec 15 01:48:19 2012 +0000
@@ -0,0 +1,74 @@
+/* mbed R/C Servo Library
+ *  
+ * Copyright (c) 2007-2010 sford, cstyles
+ *
+ * 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.
+ */
+ 
+#include "Servo.h"
+#include "mbed.h"
+
+static float clamp(float value, float min, float max) {
+    if(value < min) {
+        return min;
+    } else if(value > max) {
+        return max;
+    } else {
+        return value;
+    }
+}
+
+Servo::Servo(PinName pin) : _pwm(pin) {
+    calibrate();
+    write(0.5);
+}
+
+void Servo::write(float percent) {
+    float offset = _range * 2.0 * (percent - 0.5);
+    _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
+    _p = clamp(percent, 0.0, 1.0);
+}
+
+void Servo::position(float degrees) {
+    float offset = _range * (degrees / _degrees);
+    _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
+}
+
+void Servo::calibrate(float range, float degrees) {
+    _range = range;
+    _degrees = degrees;
+}
+
+float Servo::read() {
+    return _p;
+}
+
+Servo& Servo::operator= (float percent) { 
+    write(percent);
+    return *this;
+}
+
+Servo& Servo::operator= (Servo& rhs) {
+    write(rhs.read());
+    return *this;
+}
+
+Servo::operator float() {
+    return read();
+}

diff -r 000000000000 -r eae9712515a6 Servo/Servo.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo/Servo.h	Sat Dec 15 01:48:19 2012 +0000
@@ -0,0 +1,98 @@
+/* mbed R/C Servo Library
+ * Copyright (c) 2007-2010 sford, cstyles
+ *
+ * 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.
+ */
+  
+#ifndef MBED_SERVO_H
+#define MBED_SERVO_H
+
+#include "mbed.h"
+
+/** Servo control class, based on a PwmOut
+ *
+ * Example:
+ * @code
+ * // Continuously sweep the servo through it's full range
+ * #include "mbed.h"
+ * #include "Servo.h"
+ * 
+ * Servo myservo(p21);
+ * 
+ * int main() {
+ *     while(1) {
+ *         for(int i=0; i<100; i++) {
+ *             myservo = i/100.0;
+ *             wait(0.01);
+ *         }
+ *         for(int i=100; i>0; i--) {
+ *             myservo = i/100.0;
+ *             wait(0.01);
+ *         }
+ *     }
+ * }
+ * @endcode
+ */
+class Servo {
+
+public:
+    /** Create a servo object connected to the specified PwmOut pin
+     *
+     * @param pin PwmOut pin to connect to 
+     */
+    Servo(PinName pin);
+    
+    /** Set the servo position, normalised to it's full range
+     *
+     * @param percent A normalised number 0.0-1.0 to represent the full range.
+     */
+    void write(float percent);
+    
+    /**  Read the servo motors current position
+     *
+     * @param returns A normalised number 0.0-1.0  representing the full range.
+     */
+    float read();
+    
+    /** Set the servo position
+     *
+     * @param degrees Servo position in degrees
+     */
+    void position(float degrees);
+    
+    /**  Allows calibration of the range and angles for a particular servo
+     *
+     * @param range Pulsewidth range from center (1.5ms) to maximum/minimum position in seconds
+     * @param degrees Angle from centre to maximum/minimum position in degrees
+     */
+    void calibrate(float range = 0.0005, float degrees = 45.0); 
+        
+    /**  Shorthand for the write and read functions */
+    Servo& operator= (float percent);
+    Servo& operator= (Servo& rhs);
+    operator float();
+
+protected:
+    PwmOut _pwm;
+    float _range;
+    float _degrees;
+    float _p;
+};
+
+#endif

diff -r 000000000000 -r eae9712515a6 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Dec 15 01:48:19 2012 +0000
@@ -0,0 +1,226 @@
+#include "mbed.h"
+#include "motordriver.h"
+#include "Servo.h"
+#include "HMC6352.h"
+
+HMC6352 compass(p9, p10);
+//Smartbot
+
+DigitalOut mbedled1(LED1); //signals draw mode
+DigitalOut mbedled2(LED2); //singals relocate mode
+DigitalOut mbedled3(LED2); //signals left wheel movement
+DigitalOut mbedled4(LED2); //signals right wheel movement
+
+Serial pc(USBTX, USBRX); // tx, rx
+Serial controller(p13, p14); //change to whatever rcv method we use
+
+//See http://mbed.org/cookbook/Motor
+//http://mbed.org/users/4180_1/notebook/sparkfun-magician-robot-base-kit/
+//Connections to dual H-brdige driver for the two drive motors
+Motor left(p21, p22, p20, 1); // pwm, fwd, rev, has brake feature
+Motor right(p24, p25, p27, 1);
+
+Servo penservo(p23);
+
+int main()
+{
+    //init command vars
+    char command = 0; //bit0 = 1 if shake; bit7 = 0 if y, 1 if x; bit6 = 0 if pos, 1 if neg
+    char XorY = 128; // X or Y move command bitmask
+    char NorP = 64; // Down or Up move command bitmask
+    char mag = 62; // bitmask for magnitude of move command
+    char magnitude = 0; //magnitude of move
+
+    //Direction and Movement Variables, start on page 0 facing +y with dx, dy =0
+    float nphi, myphi, dphi = 0.0; //angle math, (36)0 = +y, 90 = +x, 180 = -y, 270 = -x
+    float dy, dx = 0.0; // where on page?
+    float ny, nx = 0.0; // next move command coords
+    //int pagesize = 1050; //square page side draw length + border
+    float moveweight = 0; //length of current move order, effects speed
+    int comcount = 4; //number of move commands to add into 1 move order
+   // int flip = 0; //backwards to save time?
+    int i;
+    
+    //Init Servo
+    penservo = .5;
+
+    //Initialize LEDs
+    mbedled1 = 1;
+    mbedled2 = 0;
+
+    //Init IR Receiver
+    controller.baud(2400);
+    wait(1);
+    
+    //Init Compass to continuous mode, periodic set/reset, 20Hz measurement rate.
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    wait(3);
+    
+    //Init Compass reading to 0
+    for (i = 0; i < 5; i++) {  //Take Averaged Compass reading
+        myphi += compass.sample()/50.0;
+        wait(.05);
+    }
+    dphi = nphi - myphi;               // calc angle diff
+    while (abs(dphi) > 15) {           // if diff sig, turn to 0
+        myphi = 0.0;
+        left.speed(-.5);
+        for (i = 0; i < 3; i++) {  //Take Averaged Compass reading
+        myphi += compass.sample()/30.0;
+        wait(.01);
+        }
+        dphi = nphi - myphi;  
+        wait(.01);             
+    }         
+    right.stop(1);                   //stop wheel(s) after turn 
+    left.stop(1);              
+    wait(.5);
+    
+    //pc.printf("myphi: %f", myphi); 
+    //pc.printf("starting \n");
+
+    while(1) {
+
+        //if readable, get command
+        if(controller.readable()) {
+            mbedled2 = 1;
+            command = controller.getc(); //Serial read
+            comcount--;
+            //pc.printf("%x \n", (command));
+            mbedled2 = 0;
+        }   else if (comcount < 4) { //decrement if command countdown started even if no command read
+            command = 0;
+            comcount--;
+        }   else {
+            command = 0;
+        }
+
+        //check for shake
+        if (command==1) { //lift pen up and reposition
+            mbedled1 = 0;
+            penservo = 1;
+            wait(3);
+            dx = 0; //reset vars after reposition
+            dy = 0;
+            comcount = 5;
+            mbedled1 = 1;
+            penservo = .5;
+        }
+
+        //check for move command
+        if ((abs(command) > 1) && (comcount > 0)) {
+            magnitude = mag & command;
+            if ( (command & XorY) == 128 ) {        // if bit7 then X axis
+                if ( (command & NorP) == 64 ) {         // if bit6 then -x
+                   // nx -= (.3+magnitude/16.0)/5.0;
+                   nx -= magnitude / 4.0;
+                } else {                                //else if !bit6 then +x
+                   // nx += (.3+magnitude/16.0)/5.0;
+                   nx += magnitude / 4.0;
+                }
+            } else {                                //else if !bit7 then Y axis
+                if ( (command & NorP) == 64 ) {         // if bit6 then -y
+                   // ny -= (.3+magnitude/16.0)/5.0;
+                   ny -= magnitude / 4.0;
+                } else {                                //else if !bit6 then +y
+                   // ny += (.3+magnitude/16.0)/5.0;
+                   ny += magnitude / 4.0;
+                }
+            }
+            wait(.05);
+        } else if (comcount == 0) { //move math
+            if ((nx != 0) && (ny != 0)) {    // if both x&y change, calculate vector angle and length in polar coords
+                dx += nx;
+                dy += ny;
+                moveweight = pow(ny, 2) + pow(nx, 2);   //moveweight = r
+                moveweight = sqrt(moveweight);
+                nphi = atan2(nx, ny);                   //new phi = principal arctan from -180 to 180. x and y flipped to get angle from y to x
+                //pc.printf("POLAR myphi: %f, newphi: %f, movelength: %f \n", myphi, nphi, moveweight); 
+            } else {
+                dphi = 0;
+                moveweight = 0;
+            }
+            
+            for (i = 0; i < 3; i++) {  //Take Averaged Compass reading
+                myphi += compass.sample()/30.0;
+                wait(.01);
+            }
+            
+            if (abs(nx) < 2) {                   //if only signif. y axis move
+                moveweight = abs(ny);                        // set speed
+                if (ny < -2) {nphi = 180;}         // set angles to axis 
+                else if (ny > 2) {nphi = 0;}
+                else {nphi = myphi;
+                    moveweight = 0;}
+            }
+    
+            if (abs(ny) < 2) {                   //if only signif. x axis move
+                moveweight = abs(nx);
+                if (nx < -2) {nphi = 270;}
+                else if (nx > 2) {nphi = 90;}
+                else {nphi = myphi;
+                    moveweight = 0;}
+            }
+             
+            dphi = nphi - myphi;               // calc angle diff
+            //if (abs(dphi) > 340) { dphi = 0;}        // fix pos y axis
+            /*if ((160 < abs(dphi)) && (abs(dphi)< 200)) {
+                moveweight = -moveweight;
+                dphi = 0;}       //flip speed instead of flip around
+            if (dphi > 180) {                 // switch angle diff with mag above 180 and flip
+                nphi -= 180;
+                 moveweight = -moveweight;   //flip speed
+            }
+            if (dphi < -180) {
+                nphi += 180;
+                 moveweight = -moveweight;      //flip speed
+            }
+            */
+            
+            
+            if (abs(dphi) > 15) { //turn left
+                right.stop(1);                   //stop wheels before turn 
+                left.stop(1);
+                wait(.1);
+                while (abs(dphi) > 15) {           // if diff sig, turn to 0
+                    myphi = 0.0;
+                    left.speed(-.5);
+                    for (i = 0; i < 2; i++) { //Take quick Averaged Compass reading
+                    myphi += compass.sample()/20.0;
+                    wait(.01);}
+                    dphi = nphi - myphi;               
+                }         
+                right.stop(1);                   //stop wheels after turn 
+                left.stop(1);              
+                wait(.1);
+            }                         
+            
+            while (moveweight >= 1) {   //go fwd for r
+                left.speed(.7);  
+                right.speed(.7);
+                wait(.1);
+                moveweight--;
+                //moveweight = moveweight/2;
+            }
+            
+            while (moveweight <= -1) {   //go back for r
+                left.speed(-.7);  
+                right.speed(-.7);
+                wait(.1);
+                moveweight++;
+                //moveweight = moveweight/2;
+            }
+    
+            comcount = 4;            //reset vars
+            nx = 0;
+            ny = 0;
+            moveweight = 0;
+            dphi = 0;
+
+        } else { //else stop
+            left.stop(1);
+            right.stop(1);
+            wait(.1);
+        }
+    }
+}

diff -r 000000000000 -r eae9712515a6 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Dec 15 01:48:19 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/b60934f96c0c
\ No newline at end of file