# blueNumber.py - Send numbers to bluetooth serial to control robot.
# (c) Kimmo Karvinen & Tero Karvinen http://sulautetut.fi
 
import appuifw, e32, key_codes, socket
 
btDeviceAddress='00:07:80:83:ab:6a' # Arduino BT. 'hcitool scan'
 
def quit():
        sock.close()
        appuifw.note(u"Bye! See you at www.sulautetut.fi!")
        appLock.signal()
 
## Main
 
canvas = appuifw.Canvas()
appuifw.app.body = canvas
 
canvas.bind(key_codes.EKey4, lambda: sock.send("4") )
canvas.bind(key_codes.EKey5, lambda: sock.send("5") )
canvas.bind(key_codes.EKey6, lambda: sock.send("6") )
# mobile phone number pad is upside down computer numpad:
canvas.bind(key_codes.EKey8, lambda: sock.send("2") ) # back 8-2
canvas.bind(key_codes.EKey2, lambda: sock.send("8") ) # forward 2-8
 
sock=socket.socket(socket.AF_BT, socket.SOCK_STREAM)
sock.connect((btDeviceAddress, 1)) # extra parenthesis for tuple
appuifw.note(u"Use 2468 to move, 5 to kick. See you at www.sulautetut.fi!")
 
appuifw.app.exit_key_handler = quit
appLock = e32.Ao_lock()
appLock.wait()

# sockerWheel.py - control soccerbot with n95 acceleration
# (c) Karvinen

import socket, e32, time, axyz, appuifw, graphics

btAddress='00:07:80:83:ab:6a'
gui=True
bg=graphics.Image.open("e:\\python\\blueServo\\bgGreen.jpg")

canvas=None
sock=False
counter=0
prevSpeed=0
appLock = e32.Ao_lock()

def suSend(left, right, kick="-"):
        print("left: "+str(left)+"      right:"+str(right))
        cmd="S"+chr(left+60)+chr(right+60)+kick+"U"
        sock.send(cmd)

def scale(x, old, new):
        (oldMin, oldMax) = old
        (newMin, newMax) = new
        p=float(x-oldMin)/(oldMax-oldMin) # x percentage of old scale
        y=(newMax-newMin)*p+newMin
        y=constrain(y, -1, 1)
        return y

def constrain(x, min, max):
        if x<min: return min
        elif max<x: return max
        else: return x

def readAcc(x, y, z):
        global counter, prevSpeed
        if counter>=10:
                left=scale(y, (-50, 50), (-1, 1))
                right=-left
                speed=scale(-z, (-50, 50), (-1, 1))
                left+=speed
                right+=speed
                left=constrain(left, -1, 1)
                right=constrain(right, -1, 1)
                if left+right<0:
                        tmp=left
                        left=right
                        right=tmp
                if (abs(left)+abs(right))/2<0.15:
                        left=0
                        right=0
                #if prevSpeed<-0.1 and speed>0.8:
                if abs(prevSpeed-speed)>0.7:
                        kick="k"
                        print("Kick!")
                        counter=-3000
                else:
                        kick="-"
                suSend(left*60, right*60, kick)
                prevSpeed=speed
                counter=0
        counter+=1

def quit():
        suSend(0, 0)
        sock.close()
        axyz.disconnect()
        appLock.signal()
        print("Closed socket. Bye bye, visit sulautetut.fi!")

def redraw(dummy=None):
        canvas.blit(bg)

def main():
        global sock, canvas
        print("Started.")
        appuifw.app.exit_key_handler = quit

        sock=socket.socket(socket.AF_BT, socket.SOCK_STREAM)
        sock.connect((btAddress, 1))
        print("Connected")
       
        axyz.connect(readAcc)
        print("Axyz connected.")

        appuifw.app.screen='full'
        canvas = appuifw.Canvas(redraw_callback=redraw)
        appuifw.app.body=canvas
        redraw()

        print("Entering main loop")
        appLock.wait()

if __name__== '__main__' :
        main()
        print("Program finnished")

# axyzPrint.py - Print acceleration from N95 accelerometer
# (c) Kimmo Karvinen & Tero Karvinen http://sulautetut.fi
 
import appuifw, e32, axyz
 
def readAcc(x, y, z):
        print x, y, z
 
def quit():
        print("Bye... visit sulautetut.fi")
        axyz.disconnect()
        app_lock.signal()
 
axyz.connect(readAcc)
 
appuifw.app.title = u"Print Acceleration - Sulautetut.fi"
appuifw.app.exit_key_handler = quit
app_lock = e32.Ao_lock()
app_lock.wait()

// centerFullRotation.pde - Turn full rotation servo clockwise,
// counterclockwise and stop.
// (c) Kimmo Karvinen & Tero Karvinen http://sulautetut.fi
 
int servoPin = 2;
 
void pulseServo(int servoPin, int pulseLenUs)
{
  digitalWrite(servoPin, HIGH);
  delayMicroseconds(pulseLenUs);
  digitalWrite(servoPin, LOW);
  delay(20);
}
 
void setup()
{
        pinMode(servoPin, OUTPUT);
}
 
void loop()
{
    pulseServo(servoPin, 1500);
}

// fullRotation.pde - Turn full rotation servo clockwise,
// counterclockwise and stop.
// (c) Kimmo Karvinen & Tero Karvinen http://sulautetut.fi
 
int servoPin = 2;
 
void pulseServo(int servoPin, int pulseLenUs)
{
  digitalWrite(servoPin, HIGH);
  delayMicroseconds(pulseLenUs);
  digitalWrite(servoPin, LOW);
  delay(20);
}
 
void servoClockWise()
{
    for (int i=0; i<200; i++) {
          pulseServo(servoPin, 1200);
    }
}
 
 void servoCounterClockWise()
 {
      for (int i=0; i<200; i++) {
          pulseServo(servoPin, 1800);
    }
 }
 
 void servoStop()
 {
     for (int i=0; i<200; i++) {
          pulseServo(servoPin, 1500);
    }
 }
 
void setup()
{
        pinMode(servoPin, OUTPUT);
}
 
void loop()
{
    servoClockWise();
    servoStop();
    servoCounterClockWise();
    servoStop();
}

// footballBotDirections.pde - Move forward, backward, turn right and left.
// (c) Kimmo Karvinen & Tero Karvinen http://sulautetut.fi

int servoRightPin=2;
int servoLeftPin=3;
 
void pulseServo(int servoPin, int pulseLenUs)
{
  digitalWrite(servoPin, HIGH);
  delayMicroseconds(pulseLenUs);
  digitalWrite(servoPin, LOW);
  delay(20);
}
 
 
void moveForward()
{
    for (int i=0; i<=15; i++) {
      pulseServo(servoLeftPin, 1800);
      pulseServo(servoRightPin, 1200);
    }
}
 
void moveBack()
{
   for (int i=0; i<=15; i++) {
     pulseServo(servoLeftPin, 1200);
    pulseServo(servoRightPin, 1800);
    }
}
void turnLeft()
{
 
    for (int i=0; i<=15; i++) {
      pulseServo(servoLeftPin, 1200);
      pulseServo(servoRightPin, 1200);
    }
}
 
void turnRight()
{
 
    for (int i=0; i<=15; i++) {
      pulseServo(servoLeftPin, 1800);
      pulseServo(servoRightPin, 1800);
    }
}
 
void setup()
{
 
        pinMode(servoLeftPin, OUTPUT);
        pinMode(servoRightPin, OUTPUT);
 
}
 
void loop()
{
moveForward();
delay(1000);
moveBack();
delay(1000);
turnRight();
delay(1000);
turnLeft();
delay(1000);
}

// footballBotForward.pde - Move forward
// (c) Kimmo Karvinen & Tero Karvinen http://sulautetut.fi

int servoRightPin=2;
int servoLeftPin=3;
 
void pulseServo(int servoPin, int pulseLenUs)
{
  digitalWrite(servoPin, HIGH);
  delayMicroseconds(pulseLenUs);
  digitalWrite(servoPin, LOW);
  delay(20);
}
 
void moveForward()
{
    for (int i=0; i<=15; i++) {
      pulseServo(servoLeftPin, 1800);
      pulseServo(servoRightPin, 1200);
    }
}
 
void setup()
{
        pinMode(servoLeftPin, OUTPUT);
        pinMode(servoRightPin, OUTPUT);
 
}
 void loop()
{
moveForward();
delay(1000);
}

// footballBotSerialControl.pde - Call move functions from serial console.
// (c) Kimmo Karvinen & Tero Karvinen http://sulautetut.fi

int servoRightPin=2;
int servoLeftPin=3;
int servoKickPin=4;
int kickNeutral=380;
int kickKick=1500;
 
void pulseServo(int servoPin, int pulseLenUs)
{
  digitalWrite(servoPin, HIGH);
  delayMicroseconds(pulseLenUs);
  digitalWrite(servoPin, LOW);
  delay(20);
}
 
void calibrateKick()
{
    for (int i=0; i<=25; i++) {
      pulseServo(servoKickPin, kickNeutral);
    }
}
 void kick()
{
    for (int i=0; i<=10; i++) {
      pulseServo(servoKickPin, kickKick);
    }
    delay(500);
    calibrateKick();
}
 
void moveForward()
{
    for (int i=0; i<=15; i++) {
     pulseServo(servoLeftPin, 1800);
    pulseServo(servoRightPin, 1200);
    }
}

 void moveBack()
{
   for (int i=0; i<=15; i++) {
     pulseServo(servoLeftPin, 1200);
    pulseServo(servoRightPin, 1800);
    }
}
void turnLeft()
{
     for (int i=0; i<=15; i++) {
      pulseServo(servoLeftPin, 1200);
      pulseServo(servoRightPin, 1200);
    }
}
 void turnRight()
{
     for (int i=0; i<=15; i++) {
      pulseServo(servoLeftPin, 1800);
      pulseServo(servoRightPin, 1800);
    }
}
 
void setup()
{
         pinMode(servoLeftPin, OUTPUT);
        pinMode(servoRightPin, OUTPUT);
        pinMode(servoKickPin, OUTPUT);
         calibrateKick();
         Serial.begin(115200);
        Serial.println("Football robot. (c) 2008 Karvinen");
 }
 
void loop()
{
 if ( Serial.available()) {
    char ch = Serial.read();
    if ('8'==ch){
      moveForward();
      }
     else if ('2'==ch){
       moveBack();
       }
      else if ('6'==ch){
       turnRight();
       }
      else if ('4'==ch){
       turnLeft();
       }
      else if ('5'==ch){
       kick();
       }
    }
 }

// footballBotKick.pde - Kick
// (c) Kimmo Karvinen & Tero Karvinen http://sulautetut.fi

int servoKickPin=4;
 
int kickNeutral=380;
int kickKick=1500;
 
void pulseServo(int servoPin, int pulseLenUs)
{
  digitalWrite(servoPin, HIGH);
  delayMicroseconds(pulseLenUs);
  digitalWrite(servoPin, LOW);
  delay(20);
}

void calibrateKick()
{
    for (int i=0; i<=25; i++) {
      pulseServo(servoKickPin, kickNeutral);
    }
}
 
void kick()
{
    for (int i=0; i<=10; i++) {
      pulseServo(servoKickPin, kickKick);
    }
    delay(500);
    calibrateKick();
}
 
void setup()
{
 
     pinMode(servoKickPin, OUTPUT);
     calibrateKick();
}
 
void loop()
{
kick();
delay(1000);
}

// footballrobot.pde - Footballrobot for cellphone control
// (c) Kimmo Karvinen & Tero Karvinen http://sulautetut.fi
 
int servoPin = 2;
 
int servoRightPin=2;
int servoLeftPin=3;
int servoKickPin=4;
 
int kickNow=0;
int kickNeutral=380;
int kickKick=1500;
int neutral=1500;
float speedToPulse=170/60;
// maxPulse=neutral+170; minPulse=neutral-170; minSpeed=-60; maxSpeed=60
int durationLeft=0;
int durationRight=0;
 
int leftSpeed=0;
int rightSpeed=0;
 
int ledPin=13;
 
void calibrateKick()
{
    for (int i=0; i<=15; i++) {
      pulseServo(servoKickPin, kickNeutral);
      Serial.flush();
    }
}
 
void kick()
{
     for (int i=0; i<=6; i++) {
      pulseServo(servoKickPin, kickKick);
      Serial.flush();
     }
     delay(100);
  calibrateKick();
}
 
void move()
{
      for (int i=0; i<=4; i++) {
        pulseServo(servoLeftPin, (int) neutral+leftSpeed*speedToPulse);
        pulseServo(servoRightPin, (int) neutral-rightSpeed*speedToPulse);
    }
}
 
void pulseServo(int servoPin, int pulseLenUs)
{
  digitalWrite(servoPin, HIGH);
  delayMicroseconds(pulseLenUs);
  digitalWrite(servoPin, LOW);
  delay(20);
}
 
void setup()
{
        pinMode(ledPin, OUTPUT);
        digitalWrite(ledPin, HIGH);
        pinMode(servoLeftPin, OUTPUT);
        pinMode(servoRightPin, OUTPUT);
        pinMode(servoKickPin, OUTPUT);
        Serial.begin(115200);
        calibrateKick();
        // Serial.println("SockerBot initialized - sulautetut.fi");
        digitalWrite(ledPin, LOW);
}
 
void loop()
{
  Serial.print("L");
        if ( Serial.available()) {
                char chInit = Serial.read();
                if ('S'==chInit && Serial.available()) {
                          char chLeft=Serial.read();
                          char chRight=Serial.read();
                          char chKick=Serial.read();
                          char chEnd=Serial.read();
                          if ('U'==chEnd) {
                                leftSpeed  = int(chLeft)-60;
                                rightSpeed = int(chRight)-60;
                                if ('k'==chKick) {
                                   kick();
                                   Serial.flush();
                                }
                        }
                } else {
                  // Serial.println("Ignored malformed data.");
                  Serial.flush();
                }
        }
        move();
        }