moMo : version 2

img_0439.jpg

-new compass unit calcuates true north in relation to itself and waypoint (so it points in the right direction even if you spin it)
-upgraded to 32 Channel!! ETek GPS module
-4 AA batteries powers the entire circuit with 5.6V at 2700mha
-a new skeleton ( to be outfitted with a flexible skin soon )
-updated code for smoother motion, although it’s soon to become obsolete as we move into a more gestural configuration ( like Keepon ) without the deadly lead weight.


momo v3 : compass + bend test from che-wei wang on Vimeo.

moMo V2 : compass motion 02 from che-wei wang on Vimeo.

here’s the mess

#include
<math.h>
#include <stdio.h>//for sprintf

int minPulse = 550;   // Minimum servo position
int maxPulse = 2500;  // Maximum servo position
int pulse=0;
int pulseR=0;
long lastPulse = 0;    // the time in milliseconds of the last pulse
int refreshTime = 20; // the time needed in between pulses

int distance=0;
int angle=0;

long swayCount=0;
long pulseCount=0;

boolean startingUp=false;
boolean gpsConnected=true;
boolean printStuff=false;

//GPS
#include <string.h>
#include <ctype.h>
int rxPin = 0;                    // RX PIN
int txPin = 1;                    // TX TX
int byteGPS=-1;
char linea[300] = "";
char comandoGPR[7] = "$GPRMC";
int cont=0;
int bien=0;
int conta=0;
int indices[13];

//vibe
int vibeMotor0 = 0;
int vibeMotor1 = 0;
int vibeMotor2 = 0;
int vibeMotor3 = 0;
int vibe=4;
int vibeOut=0;

//servo.
double wLatitude;//=4069.0645;
double wLongitude;//=7394.1636;
float wScreenLatitude;
float wScreenLongitude;
// 796 deKalb
double myLatitude; //north south
double myLongitude; //west east
float myScreenLatitude;
float myScreenLongitude;

float diam;
float initDist;

float wayPoint[2];
float myPoint[2];
int timer=0;

//compass
//Using Arduino NG rev C board and Arduino IDE v.0008
//SDA to Analog pin 4 and SCL to analog pin 5
//No pullup resistors on SDA and SCL
//Using 3v instead of 5v to the Vcc of the compass module.

#include <wire.h>
int HMC6352Address = 0x42;
// This is calculated in the setup() function
int slaveAddress;
boolean ledState = false;
byte headingData[2];
int ii, headingValue;
long compassCount=0;
float heading;
#define NUMREADINGS 4
int compassRaw[NUMREADINGS];                // the readings from the analog input
int compassIndex = 0;                       // the index of the current reading
int compassTotal = 0;                       // the running total
int compassAverage = 0;                          // the average

void setup() {
  // Set servo pin as an output pin
  pinMode(11, OUTPUT);
  pinMode(10, OUTPUT); 

  //vibe
  pinMode(3, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(9, OUTPUT);

  pulse = minPulse;           // Set the motor position value to the minimum
  pulseR = minPulse;  

  //GPS
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);
  for (int i=0;i&lt;300;i++){       // Initialize a buffer for received data
    linea[i]=' ';
  }   

  //compass
  // Shift the device's documented slave address (0x42) 1 bit right
  // This compensates for how the TWI library only wants the
  // 7 most significant bits (with the high bit padded with 0)
  slaveAddress = HMC6352Address &gt;&gt; 1;   // This results in 0x21 as the address to pass to TWI
  Serial.begin(4800);
  Wire.begin();
  for (int i = 0; i &lt; NUMREADINGS; i++) compassRaw[i] = 0; // initialize all the readings to 0

}

void compass()
{
  // Send a "A" command to the HMC6352
  // This requests the current heading data
  Wire.beginTransmission(slaveAddress);
  Wire.send("A");              // The "Get Data" command
  Wire.endTransmission();

    if(millis()-compassCount&gt;=175){// The HMC6352 needs at least a 70us (microsecond) delay
    // Read the 2 heading bytes, MSB first
    // The resulting 16bit word is the compass heading in 10th's of a degree
    // For example: a heading of 1345 would be 134.5 degrees
    Wire.requestFrom(slaveAddress, 2);        // Request the 2 byte heading (MSB comes first)
    ii = 0;
    while(Wire.available() &amp;&amp; ii &lt; 2)
    {
      headingData[ii] = Wire.receive();
      ii++;
    }
    int headingRaw = headingData[0]*256 + headingData[1];  // Put the MSB and LSB together 0-3600

    compassTotal -=compassRaw[compassIndex];               // subtract the last reading
    compassRaw[compassIndex] = headingRaw; // read from the sensor
    compassTotal += compassRaw[compassIndex];               // add the reading to the total
    compassIndex = (compassIndex + 1);                    // advance to the next index

    if (compassIndex &gt;= NUMREADINGS)               // if we're at the end of the array...
      compassIndex = 0;                            // ...wrap around to the beginning

    headingValue = compassTotal / NUMREADINGS;          // calculate the average

    //Serial.print(int (headingValue / 10));     // The whole number part of the heading
    //Serial.print(".");
    //Serial.print(int (headingValue % 10));     // The fractional part of the heading
    //Serial.println(" degrees");
    //delay(500);
    //heading=(float)headingValue/3600.0;
    compassCount=millis();
  }
} 

float decimalMinutes(float l, char d){
  //Example (signal acquired): $GPGLL,4250.5589,S,14718.5084,E,092204.999,A*2D
  //latitude ddmm.mmmm
  //longitude dddmm.mmmm
  float decimal;  //ddmm.mmmm
  int ll=l/100;//dd.mmmmmmm
  float dd=floor(ll);//dd
  float mm=ll%1;// .mmmmmm
  mm=mm*10/6; // .mmmmmm in decimal
  decimal=dd+mm;
  if(d=='W'||d=='S')decimal=decimal*-1;

  //Serial.println( int(decimal*100) );
  return decimal;
}

//translate long and lat to x and y points as if on a world map
float earthCirc=40076500.0; //m circumference at equator
//float earthCirc=10000.00000;
float earthHeight=earthCirc/PI;

float longToScreen(float longitude){
  float x;
  x=earthCirc/2.0+(longitude/180.0)*(earthCirc/2.0);
  return x;
}

float latToScreen(float latitude){
  float y;
  y=earthHeight/2.0-(latitude/90.0)*(earthHeight/2.0);
  return y;
}

float dist(float x,float y, float xx, float yy){
  float distance=sqrt((x-xx)*(x-xx)+(y-yy)*(y-yy));
  return distance;
}

void readGPS() {
  byteGPS=Serial.read();         // Read a byte of the serial port
  if (byteGPS == -1) {           // See if the port is empty yet
    // delay(100);
    gpsConnected=false;
  }
  else {
    gpsConnected=true;

    linea[conta]=byteGPS;        // If there is serial port data, it is put in the buffer
    conta++;
    // printByte(byteGPS); //print raw rx
    if (byteGPS==13){            // If the received byte is = to 13, end of transmission
      cont=0;
      bien=0;
      for (int i=1;i&lt;7;i++){     // Verifies if the received command starts with $GPR
        if (linea[i]==comandoGPR[i-1]) bien++;
      }
      if(bien==6){               // If yes, continue and process the data
        for (int i=0;i&lt;300;i++){
          if (linea[i]==','){    // check for the position of the  "," separator
            indices[cont]=i;
            cont++;
          }
          if (linea[i]=='*'){    // ... and the "*"
            indices[12]=i;
            cont++;
          }
        }
        if(printStuff) Serial.println("---------------");

        // for (int i=0;i&lt;12;i++){
        /*if(printStuff){
         switch(i){
         //case 0 :Serial.print("Time in UTC (HhMmSs): ");break;
         //case 1 :Serial.print("Status (A=OK,V=KO): ");break;
         case 2 :
         Serial.print("Latitude: ");
         break;
         case 3 :
         Serial.print("Direction (N/S): ");
         break;
         case 4 :
         Serial.print("Longitude: ");
         break;
         case 5 :
         Serial.print("Direction (E/W): ");
         break;
         //case 6 :Serial.print("Velocity in knots: ");break;
         //case 7 :Serial.print("Heading in degrees: ");break;
         //case 8 :Serial.print("Date UTC (DdMmAa): ");break;
         //case 9 :Serial.print("Magnetic degrees: ");break;
         //case 10 :Serial.print("(E/W): ");break;
         //case 11 :Serial.print("Mode: ");break;
         // case 12 :Serial.print("Checksum: ");break;

         }
         }
         */
        char NS;
        char tempLat[20];
        char resultLat[20];
        int tempLatCount=0;

        //latitude
        for (int j=indices[2];j&lt;(indices[3]-1);j++){
          //print raw
          if(printStuff) Serial.print(linea[j+1]);
          tempLat[tempLatCount]=linea[j+1];
          tempLatCount++;
        }
        if(printStuff) Serial.println("");

        for (int j=indices[3];j&lt;(indices[4]-1);j++){
          if(printStuff) Serial.println(linea[j+1]);
          NS=linea[j+1];
        }

        myLatitude=atof(tempLat);
        myLatitude=decimalMinutes(myLatitude,NS);
        myScreenLatitude=latToScreen(myLatitude);

        int resultLatInt=myLatitude;

        if(printStuff){
          Serial.print("tempLat: ");
          Serial.println(tempLat);
          Serial.print("myLatitude: ");
          Serial.println(resultLatInt);
        } 

        //longitude
        char EW;
        char tempLong[20];
        char resultLong[20];
        int tempLongCount=0;

        for (int j=indices[4];j&lt;(indices[5]-1);j++){
          //print raw
          if(printStuff) Serial.print(linea[j+1]);
          tempLong[tempLongCount]=linea[j+1];
          tempLongCount++;
        }
        if(printStuff) Serial.println("");

        for (int j=indices[5];j&lt;(indices[6]-1);j++){
          if(printStuff) Serial.println(linea[j+1]);
          EW=linea[j+1];
        }

        myLongitude=atof(tempLong);
        myLongitude=decimalMinutes(myLongitude,EW);
        myScreenLongitude=longToScreen(myLongitude);

        int resultLongInt=myLongitude;

        if(printStuff){
          Serial.print("tempLong: ");
          Serial.println(tempLong);
          Serial.print("myLongitude: ");
          Serial.println(resultLongInt);
        } 

        if(printStuff) Serial.println("---------------");

        if (startingUp){
          initDist= dist(myScreenLongitude,myScreenLatitude,wScreenLongitude,wScreenLatitude);
          Serial.print("$PMTK604*6D<cr><lf>");//query version info of FW
          Serial.print("$PMTK301,1*2D<cr><lf>");//turn on WAAS
          Serial.print("$PMTK513,1*28<cr><lf>");//turn on SBAS satelite search
          startingUp=false;

        }
      }
      conta=0;                    // Reset the buffer
      for (int i=0;i&lt;300;i++){
        linea[i]=' ';
      }
    }
  }
}

void loop() {

  //wLatitude = ll.getLat();
  //wLongitude = ll.getLon();

  //myLatitude=gpsReadLat();
  //myLongitude=gpsReadLong();

  //itp 40.729308,-73.993521
  //myLatitude=40.726055;
  //myLongitude=-73.993521;

  //kristin's 40.728745,-73.98607
  // wLatitude=40.726055;
  //wLongitude=-73.983661;

  //washington square park
  wLatitude=37.0625;
  wLongitude=-95.677068;
  wScreenLatitude=latToScreen(myLatitude);
  wScreenLongitude=longToScreen(myLongitude);
  //initDist= dist(myLongitude,myLatitude,wLongitude,wLatitude);//for testing without gps startup

  //pots
  //myLatitude=analogRead(0)/5.6833-90;//-90 tp 90
  //myLongitude=analogRead(1)/2.816-180;//-90 tp 90//180 to -180

  //int xx=myLatitude;
  //int yy=myLongitude;

  //Serial.print("lat");
  //Serial.println( xx );
  //Serial.println(" " );
  //Serial.print("long");
  //Serial.println( yy );

  readGPS();
  compass();

  //servo
  //float a = atan2(wLongitude-myLongitude, wLatitude-myLatitude)/TWO_PI-headingValue; //float from 0-1
  int a = atan2(wLongitude-myLongitude, wLatitude-myLatitude)/TWO_PI*3600+1800; //float from 0 to 1
  //Serial.println(a);
  int aa;
  aa=headingValue+a;//translate angle to 0-360
  if(aa&gt;3600) angle=aa-3600;//if angle + heading is greater than 360 use remainder
  else angle=aa;
  //Serial.println(angle);

  float d1=dist(myLongitude,myLatitude,wLongitude,wLatitude)/initDist; // float from 0-1
  //distance=int(d1*1023.0); //translate distance to 0-1023

  //simlulate 360 rotation with 180 servo rotation
  int servoAngle;
  if(angle&lt;1800){
      distance=1023.0; //translate distance to 0-1023
    servoAngle=((float)angle/1800.0)*1023.0;
  }
  else if(angle&gt;=1800){
    distance=0;
    servoAngle=(((float)angle-1800 )/1800.0 )*1023.0;
  }
  // Serial.println(servoAngle);

  /*
    Serial.print("angle: ");
   Serial.println(servoAngle);
   Serial.print("dist: ");
   Serial.println(distance);
   Serial.print("initial dist: ");
   Serial.println( int(initDist*100) );
   */

  int vibeSpeed=int(d1);
  int calmTime;
  //gpsConnected=true;

  //if(gpsConnected==true){
  //swing
  /*
    //750millisec per heartbeat - 200 based on distance = faster heartbeat as dist gets smaller
   if(distance&gt;512){
   calmTime=850-(1023-distance)/512.0*200;
   } //750millisec per heartbeat - 200 based on distance = faster heartbeat as dist gets smaller
   else{
   calmTime=850-(distance)/512.0*200;
   }

   int kickTime=100;
   //swing
   if (millis()-swayCount<calmtime){>
   if(distance&gt;512){
   pulse=(512.0 +  (distance-512.0) *(calmTime-(millis()-swayCount))/calmTime   ) *19/10+minPulse; //zero distance
   //pulse=(512)*19/10+minPulse; //zero distance
   }

   else {
   pulse=(512.0 -  (512.0-distance) *(calmTime-(millis()-swayCount))/calmTime    ) *19/10+minPulse; //zero distance
   // pulse=(512)*19/10+minPulse; //zero distance
   }
   //analogWrite(5,0);
   //analogWrite(6,0);
   }
   else  if (millis()-swayCount&gt;calmTime &amp;&amp; millis()-swayCount<calmtime+kicktime){>
   pulse = distance*19/10+minPulse;    // convert the analog value
   //analogWrite(5,170);
   //analogWrite(6,170);
   }
   else  if (millis()-swayCount&gt;calmTime+kickTime ){

   swayCount=millis();
   }

   */
  pulse = distance/1023.0*(float)(maxPulse-minPulse)+minPulse;    // convert the analog value
  //}

  //no GPS signal

  /*
  else{
   int pulseTime=1000;
   // pulse = 512*19/10+minPulse;    // zero

   analogWrite(5,0);
   analogWrite(6,0);
   if (millis()-pulseCount&gt;pulseTime){
   // analogWrite(5,180);
   // analogWrite(6,180);
   }
   else if (millis()-pulseCount&gt;pulseTime){
   pulseCount=millis();
   }
   }
   */

  pulseR = servoAngle/1023.0*(float)(maxPulse-minPulse)+minPulse;    // convert the analog value

    // pulse the servo again if rhe refresh time (20 ms) have passed:
  if (millis() - lastPulse &gt;= refreshTime) {
    digitalWrite(11, HIGH);  // Turn the motor on
    delayMicroseconds(pulseR);       // Length of the pulse sets the motor position
    digitalWrite(11, LOW);    // Turn the motor off
    digitalWrite(10, HIGH);   // Turn the motor on
    delayMicroseconds(pulse);       // Length of the pulse sets the motor position
    digitalWrite(10, LOW);    // Turn the motor off
    lastPulse = millis();           // save the time of the last pulse
  }
  //Serial.print(255,BYTE); //send something

}
</calmtime+kicktime){></calmtime){></lf></cr></lf></cr></lf></cr></wire.h></ctype.h></string.h></stdio.h></math.h>