-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
#include //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
#include
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
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<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 >> 1; // This results in 0x21 as the address to pass to TWI
Serial.begin(4800);
Wire.begin();
for (int i = 0; i < 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>=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() && ii < 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 >= 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<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<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<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<(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<(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<(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<(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");//query version info of FW
Serial.print("$PMTK301,1*2D");//turn on WAAS
Serial.print("$PMTK513,1*28");//turn on SBAS satelite search
startingUp=false;
}
}
conta=0; // Reset the buffer
for (int i=0;i<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>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<1800){
distance=1023.0; //translate distance to 0-1023
servoAngle=((float)angle/1800.0)*1023.0;
}
else if(angle>=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>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
if(distance>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>calmTime && millis()-swayCount
pulse = distance*19/10+minPulse; // convert the analog value
//analogWrite(5,170);
//analogWrite(6,170);
}
else if (millis()-swayCount>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>pulseTime){
// analogWrite(5,180);
// analogWrite(6,180);
}
else if (millis()-pulseCount>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 >= 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
}
){> ){>