ottt机器人代码

//  OTTO WITH ARMS
//----------------------------------------------------------------
//-- Zowi basic firmware v2 adapted to Otto
//-- (c) BQ. Released under a GPL licencse
//-- 04 December 2015
//-- Original Authors:  Anita de Prado: ana.deprado@bq.com
//--           Jose Alberca:   jose.alberca@bq.com
//--           Javier Isabel:  javier.isabel@bq.com
//--           Juan Gonzalez (obijuan): juan.gonzalez@bq.com
//--           Irene Sanz : irene.sanz@bq.com
//-----------------------------------------------------------------
//-- Jason Snow
//-- AUGUST 2018: modified for OTTO ARMS
//-- Jason Snow
//-----------------------------------------------------------------


#include <BatReader.h>
#include <US.h>
#include "MaxMatrix.h"
MaxMatrix ledmatrix=MaxMatrix(12,10,11, 1); //PIN  12=DIN, PIN 10=CS, PIN 11=CLK

//-- Library to manage serial commands
#include <OttoSerialCommand.h>
OttoSerialCommand SCmd;  //The SerialCommand object

//-- Otto Library
#include <OttoARM.h>
Otto Otto;  //This is Otto!
 
//---------------------------------------------------------
//-- First step: Configure the pins where the servos are attached
/*
         --------------- 
        |     O   O     |
        |---------------|
RA 7==> |               | <== LA 6
        |               |
LR 3==> |               | <== LL 2
         --------------- 
            ||     ||
            ||     ||
FR 5==>   -----   ------  <== FL 4
         |-----   ------|
*/

 // #define PIN_YL 2 //servo[0]
  //#define PIN_YR 3 //servo[1]
  //#define PIN_RL 4 //servo[2]
  //#define PIN_RR 5 //servo[3]
  //#define PIN_LA 6 //servo[4]
  //#define PIN_RA 7 //servo[5]

// buzzer connected to pin 13

///
//-- Global Variables -------------------------------------------//
///

const char programID[]="ARM2"; //Each program will have a ID


//-- Movement parameters
int T=1000;              //Initial duration of movement
int moveId=0;            //Number of movement
int modeId=0;            //Number of mode
int moveSize=15;         //Asociated with the height of some movements

//---------------------------------------------------------
//-- Otto has 5 modes:
//--    * MODE = 0: Otto is awaiting  
//--    * MODE = 1: Dancing mode!  
//--    * MODE = 2: Obstacle detector mode  
//--    * MODE = 3: Noise detector mode   
//--    * MODE = 4: OttoPAD or any Teleoperation mode (listening SerialPort). 
//---------------------------------------------------------
volatile int MODE=3; //State of Otto in the principal state machine. 

unsigned long previousMillis=0;

int randomDance=0;
int randomSteps=0;

bool obstacleDetected = false;

///
//-- Setup ------------------------------------------------------//
///
void setup(){
  //Serial communication initialization
  Serial.begin(9600);  
  //Set the servo pins
  Otto.init(2,3,4,5,6,7,true);
  // set up Matrix display
  ledmatrix.init();
  ledmatrix.setIntensity(1);
  //Set a random seed
  randomSeed(analogRead(A6));
  //Setup callbacks for SerialCommand commands 
  SCmd.addCommand("S", receiveStop);      //  sendAck & sendFinalAck
  SCmd.addCommand("L", receiveLED);       //  sendAck & sendFinalAck
  SCmd.addCommand("M", receiveMovement);  //  sendAck & sendFinalAck
  SCmd.addCommand("H", receiveGesture);   //  sendAck & sendFinalAck
  SCmd.addCommand("K", receiveSing);      //  sendAck & sendFinalAck
  SCmd.addCommand("C", receiveTrims);     //  sendAck & sendFinalAck
  SCmd.addCommand("G", receiveServo);     //  sendAck & sendFinalAck
  SCmd.addCommand("D", requestDistance);
  SCmd.addCommand("N", requestNoise);
  SCmd.addCommand("B", requestBattery);   // 
  SCmd.addCommand("I", requestProgramId);
  SCmd.addCommand("J", requestMode);
  SCmd.addDefaultHandler(receiveStop);
  Otto.sing(S_connection);
  Otto.home();
 // Animation - A little moment of initial surprise
 //-----
  for(int i=0; i<2; i++){
      for (int i=0;i<4;i++){  
        Otto.putAnimationMouth(dreamMouth,i);
        delay(150);
      }
  }
  //Smile for a happy Otto :)
    Otto.putMouth(smile);
    Otto.sing(S_happy);
    delay(200);

  Otto.putMouth(happyOpen);
  //Otto.putMouth(xMouth);    //test
  previousMillis = millis();
  Otto.home();

}
///
//-- Principal Loop ---------------------------------------------//
///
void loop() {
  if (Serial.available()>0 && MODE!=4){
  // test
    SCmd.readSerial();
    //MODE=4;
    Otto.putMouth(happyOpen);
  }
  switch (MODE) {

      //-- MODE 0 - Otto is awaiting
      //---------------------------------------------------------
      case 0:
      
        //Every 80 seconds in this mode, Otto falls asleep 
        if (millis()-previousMillis>=80000){
            OttoSleeping_withInterrupts(); //ZZzzzzz...
            previousMillis=millis();         
        }

        break;
      
      //-- MODE 1 - Dance Mode!
      //---------------------------------------------------------
      case 1:
        
        randomDance=random(5,23); //5,20
        if((randomDance>14)&&(randomDance<19)){
            randomSteps=1;
            T=1600;
        }
        else{
            randomSteps=random(3,6); //3,5
            T=1000;
        }
        
        Otto.putMouth(random(10,21));

        for (int i=0;i<randomSteps;i++){
            move(randomDance);
            
        }
        break;

      //-- MODE 2 - Obstacle detector mode
      //---------------------------------------------------------
      case 2:

        if(obstacleDetected){
              Otto.putMouth(bigSurprise);
              Otto.sing(S_surprise);
              Otto.jump(5, 500);
              Otto.putMouth(confused);
              Otto.sing(S_cuddly);
            //Otto takes two steps back
            for(int i=0;i<3;i++){ 
            
              Otto.walk(1,1300,-1);
            }

            delay(100);
            obstacleDetector();
            delay(100);
           //If there are no obstacles and no button is pressed, Otto shows a smile
           if(obstacleDetected==true){break;}            
           else{
              Otto.putMouth(smile);
              delay(50);
              obstacleDetector();
           } 
           
           //If there are no obstacles and no button is pressed, Otto shows turns left
           for(int i=0; i<3; i++){
              if(obstacleDetected==true){break;}            
              else{ 
                  Otto.turn(1,1000,1); 
                  obstacleDetector();
              } 
           }
            
            //If there are no obstacles and no button is pressed, Otto is happy
            if(obstacleDetected==true){break;}           
            else{
                Otto.home();
                Otto.putMouth(happyOpen);
                Otto.sing(S_happy_short);
                delay(200);
            }     
        
        }else{

            Otto.walk(1,1000,1); //Otto walk straight
            obstacleDetector();
        }   

        break;

      //-- MODE 3 - Noise detector mode
      //---------------------------------------------------------  
      case 3:
        if (Otto.getNoise()>=650){ //740
          
          delay(50);  //Wait for the possible 'lag' of the button interruptions. 
                      //Sometimes, the noise sensor detect the button before the interruption takes efect 
 
            Otto.putMouth(bigSurprise);
            Otto.sing(S_OhOoh);
            Otto.putMouth(random(10,21));
            randomDance=random(5,21);
            move(randomDance);
            Otto.home();
            delay(500); //Wait for possible noise of the servos while get home
            Otto.putMouth(happyOpen);
        }
        break;
        
      //-- MODE 4 - OttoPAD or any Teleoperation mode (listening SerialPort) 
      //---------------------------------------------------------
      case 4:

        SCmd.readSerial();
        
        //If Otto is moving yet
        if (Otto.getRestState()==false){  
          move(moveId);
        }
      
        break;      

      default:
          MODE=0;
          break;
    }
}  

///
//-- Functions --------------------------------------------------//
///

//-- Function to read distance sensor & to actualize obstacleDetected variable
void obstacleDetector(){

   int distance = Otto.getDistance();

        if(distance<15){
          obstacleDetected = true;
        }else{
          obstacleDetected = false;
        }
}

//-- Function to receive Stop command.
void receiveStop(){

    sendAck();
    Otto.home();
    sendFinalAck();

}

//-- Function to receive LED commands
void receiveLED(){  

    //sendAck & stop if necessary
    sendAck();
    Otto.home();

    //Examples of receiveLED Bluetooth commands
    //L 000000001000010100100011000000000
    //L 001111111111111111111111111111111 (todos los LED encendidos)
    unsigned long int matrix;
    char *arg;
    char *endstr;
    arg=SCmd.next();
    //Serial.println (arg);
    if (arg != NULL) {
      matrix=strtoul(arg,&endstr,2);    // Converts a char string to unsigned long integer
      Otto.putMouth(matrix,false);
    }else{
      Otto.putMouth(xMouth);
      delay(2000);
      Otto.clearMouth();
    }

    sendFinalAck();

}



//-- Function to receive TRims commands
void receiveTrims(){  

    //sendAck & stop if necessary
    sendAck();
    Otto.home(); 

    int trim_YL,trim_YR,trim_RL,trim_RR,trim_LA,trim_RA;

    //Definition of Servo Bluetooth command
    //C trim_YL trim_YR trim_RL trim_RR
    //Examples of receiveTrims Bluetooth commands
    //C 20 0 -8 3
    bool error = false;
    char *arg;
    arg=SCmd.next();
    if (arg != NULL) { trim_YL=atoi(arg); }    // Converts a char string to an integer   
    else {error=true;}

    arg = SCmd.next(); 
    if (arg != NULL) { trim_YR=atoi(arg); }    // Converts a char string to an integer  
    else {error=true;}

    arg = SCmd.next(); 
    if (arg != NULL) { trim_RL=atoi(arg); }    // Converts a char string to an integer  
    else {error=true;}

    arg = SCmd.next(); 
    if (arg != NULL) { trim_RR=atoi(arg); }    // Converts a char string to an integer  
    else {error=true;}

    arg = SCmd.next(); 
    if (arg != NULL) { trim_LA=atoi(arg); }    // Converts a char string to an integer  
    else {error=true;}
    
    arg = SCmd.next(); 
    if (arg != NULL) { trim_RA=atoi(arg); }    // Converts a char string to an integer  
    else {error=true;}
    
    if(error==true){

      Otto.putMouth(xMouth);
      delay(2000);
      Otto.clearMouth();

    }else{ //Save it on EEPROM
      Otto.setTrims(trim_YL, trim_YR, trim_RL, trim_RR, trim_LA, trim_RA);
      Otto.saveTrimsOnEEPROM(); //Uncomment this only for one upload when you finaly set the trims.
    } 

    sendFinalAck();

}


//-- Function to receive Servo commands
void receiveServo(){  

    sendAck(); 
    moveId = 30;

    //Definition of Servo Bluetooth command
    //G  servo_YL servo_YR servo_RL servo_RR 
    //Example of receiveServo Bluetooth commands
    //G 90 85 96 78 
    bool error = false;
    char *arg;
    int servo_YL,servo_YR,servo_RL,servo_RR,servo_LA,servo_RA;

    arg=SCmd.next();
    if (arg != NULL) { servo_YL=atoi(arg); }    // Converts a char string to an integer   
    else {error=true;}

    arg = SCmd.next(); 
    if (arg != NULL) { servo_YR=atoi(arg); }    // Converts a char string to an integer  
    else {error=true;}

    arg = SCmd.next(); 
    if (arg != NULL) { servo_RL=atoi(arg); }    // Converts a char string to an integer  
    else {error=true;}

    arg = SCmd.next(); 
    if (arg != NULL) { servo_RR=atoi(arg); }    // Converts a char string to an integer  
    else {error=true;}

    arg = SCmd.next(); 
    if (arg != NULL) { servo_LA=atoi(arg); }    // Converts a char string to an integer  
    else {error=true;}

    arg = SCmd.next(); 
    if (arg != NULL) { servo_RA=atoi(arg); }    // Converts a char string to an integer  
    else {error=true;}
    
    if(error==true){

      Otto.putMouth(xMouth);
      delay(2000);
      Otto.clearMouth();

    }else{ //Update Servo:

      int servoPos[6]={servo_YL, servo_YR, servo_RL, servo_RR, servo_LA, servo_RA}; 
      Otto._moveServos(200, servoPos);   //Move 200ms
      
    }

    sendFinalAck();

}


//-- Function to receive movement commands
void receiveMovement(){

    sendAck();

    if (Otto.getRestState()==true){
        Otto.setRestState(false);
    }

    //Definition of Movement Bluetooth commands
    //M  MoveID  T   MoveSize  
    char *arg; 
    arg = SCmd.next(); 
    if (arg != NULL) {moveId=atoi(arg);}
    else{
      Otto.putMouth(xMouth);
      delay(2000);
      Otto.clearMouth();
      moveId=0; //stop
    }
    
    arg = SCmd.next(); 
    if (arg != NULL) {T=atoi(arg);}
    else{
      T=1000;
    }

    arg = SCmd.next(); 
    if (arg != NULL) {moveSize=atoi(arg);}
    else{
      moveSize =15;
    }
}


//-- Function to execute the right movement according the movement command received.
void move(int moveId){

  bool manualMode = false;

  switch (moveId) {
    case 0:
      Otto.home();
      break;
    case 1: //M 1 1000 
      Otto.walk(1,T,1);
      break;
    case 2: //M 2 1000 
      Otto.walk(1,T,-1);
      break;
    case 3: //M 3 1000 
      Otto.turn(1,T,1);
      break;
    case 4: //M 4 1000 
      Otto.turn(1,T,-1);
      break;
    case 5: //M 5 1000 30 
      Otto.updown(1,T,moveSize);
      break;
    case 6: //M 6 1000 30
      Otto.moonwalker(1,T,moveSize,1);
      break;
    case 7: //M 7 1000 30
      Otto.moonwalker(1,T,moveSize,-1);
      break;
    case 8: //M 8 1000 30
      Otto.swing(1,T,moveSize);
      break;
    case 9: //M 9 1000 30 
      Otto.crusaito(1,T,moveSize,1);
      break;
    case 10: //M 10 1000 30 
      Otto.crusaito(1,T,moveSize,-1);
      break;
    case 11: //M 11 1000 
      Otto.jump(1,T);
      break;
    case 12: //M 12 1000 30 
      Otto.flapping(1,T,moveSize,1);
      break;
    case 13: //M 13 1000 30
      Otto.flapping(1,T,moveSize,-1);
      break;
    case 14: //M 14 1000 20
      Otto.tiptoeSwing(1,T,moveSize);
      break;
    case 15: //M 15 500 
      Otto.bend(1,T,1);
      break;
    case 16: //M 16 500 
      Otto.bend(1,T,-1);
      break;
    case 17: //M 17 500 
      Otto.shakeLeg(1,T,1);
      break;
    case 18: //M 18 500 
      Otto.shakeLeg(1,T,-1);
      break;
    case 19: //M 19 500 20
      Otto.jitter(1,T,moveSize);
      break;
    case 20: //M 20 500 15
      Otto.ascendingTurn(1,T,moveSize);
      break;
      case 21: //M 21
      Otto.handsup();
      break;
      case 22: //M 22 right arm
      Otto.handwave(1);
      break;
      case 23: //M 23 left arm
      Otto.handwave(-1);
      break;
    default:
        manualMode = true;
      break;
  }

  if(!manualMode){
    sendFinalAck();
  }
       
}


//-- Function to receive gesture commands
void receiveGesture(){

    //sendAck & stop if necessary
    sendAck();
    Otto.home(); 

    //Definition of Gesture Bluetooth commands
    //H  GestureID  
    int gesture = 0;
    char *arg; 
    arg = SCmd.next(); 
    if (arg != NULL) {gesture=atoi(arg);}
    else 
    {
      //Otto.putMouth(xMouth);
      delay(2000);
      //Otto.clearMouth();
    }

    switch (gesture) {
      case 1: //H 1 
        Otto.playGesture(OttoHappy);
        break;
      case 2: //H 2 
        Otto.playGesture(OttoSuperHappy);
        break;
      case 3: //H 3 
        Otto.playGesture(OttoSad);
        break;
      case 4: //H 4 
        Otto.playGesture(OttoSleeping);
        break;
      case 5: //H 5  
        Otto.playGesture(OttoFart);
        break;
      case 6: //H 6 
        Otto.playGesture(OttoConfused);
        break;
      case 7: //H 7 
        Otto.playGesture(OttoLove);
        break;
      case 8: //H 8 
        Otto.playGesture(OttoAngry);
        break;
      case 9: //H 9  
        Otto.playGesture(OttoFretful);
        break;
      case 10: //H 10
        Otto.playGesture(OttoMagic);
        break;  
      case 11: //H 11
        Otto.playGesture(OttoWave);
        break;   
      case 12: //H 12
        Otto.playGesture(OttoVictory);
        break; 
      case 13: //H 13
        Otto.playGesture(OttoFail);
        break;         
      default:
        break;
    }

    sendFinalAck();
}

//-- Function to receive sing commands
void receiveSing(){

    //sendAck & stop if necessary
    sendAck();
    Otto.home(); 

    //Definition of Sing Bluetooth commands
    //K  SingID    
    int sing = 0;
    char *arg; 
    arg = SCmd.next(); 
    if (arg != NULL) {sing=atoi(arg);}
    else 
    {
     // Otto.putMouth(xMouth);
      delay(2000);
      //Otto.clearMouth();
    }

    switch (sing) {
      case 1: //K 1 
        Otto.sing(S_connection);
        break;
      case 2: //K 2 
        Otto.sing(S_disconnection);
        break;
      case 3: //K 3 
        Otto.sing(S_surprise);
        break;
      case 4: //K 4 
        Otto.sing(S_OhOoh);
        break;
      case 5: //K 5  
        Otto.sing(S_OhOoh2);
        break;
      case 6: //K 6 
        Otto.sing(S_cuddly);
        break;
      case 7: //K 7 
        Otto.sing(S_sleeping);
        break;
      case 8: //K 8 
        Otto.sing(S_happy);
        break;
      case 9: //K 9  
        Otto.sing(S_superHappy);
        break;
      case 10: //K 10
        Otto.sing(S_happy_short);
        break;  
      case 11: //K 11
        Otto.sing(S_sad);
        break;   
      case 12: //K 12
        Otto.sing(S_confused);
        break; 
      case 13: //K 13
        Otto.sing(S_fart1);
        break;
      case 14: //K 14
        Otto.sing(S_fart2);
        break;
      case 15: //K 15
        Otto.sing(S_fart3);
        break;    
      case 16: //K 16
        Otto.sing(S_mode1);
        break; 
      case 17: //K 17
        Otto.sing(S_mode2);
        break; 
      case 18: //K 18
        Otto.sing(S_mode3);
        break;   
      case 19: //K 19
        Otto.sing(S_buttonPushed);
        break;                      
      default:
        break;
    }

    sendFinalAck();
}




//-- Function to send ultrasonic sensor measure (distance in "cm")
void requestDistance(){

    Otto.home();  //stop if necessary  

    int distance = Otto.getDistance();
    Serial.print(F("&&"));
    Serial.print(F("D "));
    Serial.print(distance);
    Serial.println(F("%%"));
    Serial.flush();
}


//-- Function to send noise sensor measure
void requestNoise(){

    Otto.home();  //stop if necessary

    int microphone= Otto.getNoise(); //analogRead(PIN_NoiseSensor);
    Serial.print(F("&&"));
    Serial.print(F("N "));
    Serial.print(microphone);
    Serial.println(F("%%"));
    Serial.flush();
}


//-- Function to send battery voltage percent
void requestBattery(){

    Otto.home();  //stop if necessary

    //The first read of the batery is often a wrong reading, so we will discard this value. 
    double batteryLevel = Otto.getBatteryLevel();

    Serial.print(F("&&"));
    Serial.print(F("B "));
    Serial.print(batteryLevel);
    Serial.println(F("%%"));
    Serial.flush();
}


//-- Function to send program ID
void requestProgramId(){

    Otto.home();   //stop if necessary

    Serial.print(F("&&"));
    Serial.print(F("I "));
    Serial.print(programID);
    Serial.println(F("%%"));
    Serial.flush();
}


//-- Function to send Ack comand (A)
void sendAck(){

  delay(30);

  Serial.print(F("&&"));
  Serial.print(F("A"));
  Serial.println(F("%%"));
  Serial.flush();
}


//-- Function to send final Ack comand (F)
void sendFinalAck(){

  delay(30);

  Serial.print(F("&&"));
  Serial.print(F("F"));
  Serial.println(F("%%"));
  Serial.flush();
}
//-- Function to receive mode selection.
void requestMode(){

    sendAck();
    Otto.home();
    //Definition of Mode Bluetooth commands
    //J ModeID    
    char *arg; 
    arg = SCmd.next(); 
    if (arg != NULL) 
    {
      modeId=atoi(arg);
      Otto.putMouth(heart);
      }
    else{
      Otto.putMouth(xMouth);
      delay(2000);
      Otto.clearMouth();
      modeId=0; //stop
    }
switch (modeId) {
      case 0: //
        MODE = 0;
        break;
      case 1: //
        MODE = 1;
        Otto.sing(S_mode1);
        Otto.putMouth(one);
        delay(1000);
    delay(200);
        break;
        case 2: //
        MODE = 2;
        Otto.sing(S_mode2);
        Otto.putMouth(two);
        delay(1000);
        break;
        case 3: //
        MODE = 3;
        Otto.sing(S_mode3);
        Otto.putMouth(three);
        delay(1000);        
        break;
        case 4: //
        Otto.sing(S_mode1);
        Otto.putMouth(four);
        delay(1000);       
        MODE = 4;
        break;
     default:
        MODE = 0;
        break;
   }
sendFinalAck();
 }

//-- Functions with animatics
//--------------------------------------------------------

void OttoLowBatteryAlarm(){

    double batteryLevel = Otto.getBatteryLevel();

    if(batteryLevel<45){
        
      

          Otto.putMouth(thunder);
          Otto.bendTones (880, 2000, 1.04, 8, 3);  //A5 = 880
          
          delay(30);

          Otto.bendTones (2000, 880, 1.02, 8, 3);  //A5 = 880
          Otto.clearMouth();
          delay(500);
      
    }
}

void OttoSleeping_withInterrupts(){

  int bedPos_0[6]={100, 80, 60, 120, 30, 150}; 

  
    Otto._moveServos(700, bedPos_0);  
  

  for(int i=0; i<4;i++){

    
      Otto.putAnimationMouth(dreamMouth,0);
      Otto.bendTones (100, 200, 1.04, 10, 10);
     //delay(500);
    
      Otto.putAnimationMouth(dreamMouth,1);
      Otto.bendTones (200, 300, 1.04, 10, 10);  
     //delay(500);
    
      Otto.putAnimationMouth(dreamMouth,2);
      Otto.bendTones (300, 500, 1.04, 10, 10);   

    delay(500);
    
    
      Otto.putAnimationMouth(dreamMouth,1);
      Otto.bendTones (400, 250, 1.04, 10, 1); 

     //delay(500);
      Otto.putAnimationMouth(dreamMouth,0);
      Otto.bendTones (250, 100, 1.04, 10, 1); 
    
    delay(500);
  } 

  
    Otto.putMouth(lineMouth);
    Otto.sing(S_cuddly);
  //delay(500);

  Otto.home();
  Otto.putMouth(happyOpen); 

}

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值