File content as of revision 4:039a7d1ce3e9:

#include <mbed.h>
#include <PwmServo.h>
#include <ExtendedServo.h>
#include <DelayServo.h>
//#include <RobotArm.h>
#include <def_resources.h>
#include <Serial.h>
#include "Roomba.h"
#include "EventArg.h"


/*

    Controller firmware for CaitSithDanger

    Kyoto-Densou-An 2014

    Author : yishii
*/

#define DEBUG
#define BUFFER_SIZE 11
//#define COMMAND_TOTAL 3


char receive[BUFFER_SIZE]; 
//char* p_receive;
//int recvPnt = 0;

int oyatsuCnt = 0;

EventArg e;

int serial_gets(char *buffer, int size);

void initialize();

//void moveServo()
//{
//  shakeHead.moveServo();
//  nodHead.moveServo();
//  leftArm.moveServo();
//  leftShoulder.moveServo();
//  rightArm.moveServo();
//  rightShoulder.moveServo();
//}

Ticker mTick;

Roomba nyandaBase(SERIAL_TX, SERIAL_RX, PA_14);

int main() {
  pc.baud(115200);
  initialize();
  debugled = 1;
  //mTick.attach_us(&moveServo, 10000);


    Roomba* cp = new Roomba(SERIAL_TX, SERIAL_RX, LED1);//Class pointor
    void (Roomba::*fp)(void) = &Roomba::serialReceiveCallback;
    
    //<戻り値の型> (<クラス名>::*<変数名>(<引数リスト>);
    
    //fp = cp->serialReceiveCallback();
    
    //ticker.attach(&cp, cp->serialReceiveCallback, 0.1);
    
    
    //(cp->*fp)();//クラスポインタとメンバポインタ両方を使うメソッド呼び出し？なにに使うの？
    
    //ticker.attach(&cp, (cp->*fp)(), 0.1);
    //Roomba* cp = new Roomba(SERIAL_TX, SERIAL_RX, LED1);//Class pointor
    //void (Roomba::*fp)(void) = &Roomba::serialReceiveCallback;
    
    //<戻り値の型> (<クラス名>::*<変数名>(<引数リスト>);
    
    //fp = cp->serialReceiveCallback();
    
    //ticker.attach(&cp, cp->serialReceiveCallback, 0.1);
    
    
    //(cp->*fp)();//クラスポインタとメンバポインタ両方を使うメソッド呼び出し？なにに使うの？
    
    //ticker.attach(&cp, (cp->*fp)(), 0.1);

  //mTick.attach(Roomba, &nyandaBase.periodicCallback, 0.1);


  while(1) {
    char buffer[BUFFER_SIZE];
    // コマンド受信    
    int size = serial_gets(buffer, BUFFER_SIZE);
         
    if (size > 0) {

#ifdef DEBUG
      pc.printf("%s", buffer);
#endif
      // コマンド判定
      if (strncmp(buffer, "CT", 2) == 0) {
        
        myled = !myled;

        bool ret = false;
        
        if (size == 2) {
          ret = true;
        } else if (size >= 3) {
          
            
        switch (buffer[2]) {
          case 'Z':
          
            initialize();
          
            ret = true;
            break;
            
          case 'N':
          
            ret = true;
            if (buffer[3] == '0') {
              nyandaLight = 0;
            } else if (buffer[3] == '1') {
              nyandaLight = 1;
            } else {
              ret = false;    
            }

            break;
            
          case 'M':
            
            ret = true;
            
            //CTM+500-500
            
            char buff[4+1];
            int  temp;
  
            //printf("Param:%s\n", before);
            strncpy(buff, buffer + 3, 4);
            buff[4] = '\0';
            //printf("ASC:%s\n", buff);
            temp = atoi(buff);
            e.leftWheetVelocity = temp;
            
            //printf("Param:%s\n", before);
            strncpy(buff, (char*)buffer[3+4], 4);
            buff[4] = '\0';
            //printf("ASC:%s\n", buff);
            temp = atoi(buff);
            e.rightWheelVelocity = temp;
          
            break;
              
          case 'O':
          
            ret = true;
            if (buffer[3] == '0') {
              oyatsuCnt = 0;
              oyatsuFeeder.setAngle(0);
            } else if (buffer[3] == '1') {
              if (oyatsuCnt <= 3) {
                oyatsuCnt++;
              }
              
              switch (oyatsuCnt) {
                //case 0:
                //    oyatusFeeder.setAngle(0);
                //break;
                case 1:
                    oyatsuFeeder.setAngle(60);
                break;
                
                case 2:
                    oyatsuFeeder.setAngle(90);
                break;
                
                case 3:
                    oyatsuFeeder.setAngle(120);
                break;
                
                default:
                break;
              }
            } else {
              ret = false;
            }
          
            break;
          case 'H':
          
            int servoAngle = -1;
            servoAngle = atoi((buffer+4));
            
            ret = true;
            if (buffer[3] == '1') {
              shakeHead.setAngle(servoAngle);
            } else if (buffer[3] == '2') {
              nodHead.setAngle(servoAngle);
            } else {
              ret = false;
            }
            break;
            
          case 'L':
            servoAngle = atoi((buffer+4));
            
            ret = true;
            if (buffer[3] == '1') {
              leftShoulder.setAngle(servoAngle);
            } else if (buffer[3] == '2') {
              leftArm.setAngle(servoAngle);
            } else {
              ret = false;
            }
            break;
          
          case 'R':
            servoAngle = atoi((buffer+4));

            ret = true;
            if (buffer[3] == '1') {
              rightShoulder.setAngle(servoAngle);
            } else if (buffer[3] == '2') {
              rightArm.setAngle(servoAngle);
            } else {
              ret = false;   
            }
            break;
            
          default:
            ret = false;
            break;
          }
        }
        
        if (ret) {
          pc.printf("\r\nOK\r\n");
        } else {
          pc.printf("\r\nNG\r\n");  
        }
      }
    }
    //シリアル受信待ちをwihelで待っているので、定期送信は不可能
//    nyandaBase.periodicCallback(e);
//    debugled != debugled;
//    //そもそもTickerで書ける？引数付きのメソッドをattachできるのか
//    //attach第2引数のインスタンスメソッドのポインタは引数付きとか書けるのか
//    wait_ms(100);
  }
}


void initialize() {
  nyandaLight = 0;
  
  shakeHead.setAngle(0);
  nodHead.setAngle(0);
  leftArm.setAngle(0);
  leftShoulder.setAngle(0);
  rightArm.setAngle(0);
  rightShoulder.setAngle(0);
  
}


/*
 * 
 */
int serial_gets(char *buffer, int size) {
  /*if (!pc.readable()) {
    return 0;
  }*/

  int i = 0;
  while (i < size - 1) {
    if (pc.readable()) {
      buffer[i] = pc.getc();
      //pc.printf("%c", buffer[i]);
      
      if (buffer[i] == '\r') {
        --i;
      }
      if (buffer[i] == '\n') {
      
        break;
      }
      ++i;
    }/* else {
      wait_ms(1);
    }*/
  }
  buffer[i] = '\0';
  //pc.printf("%s", buffer);
  return i;
}



///*/*
// *
// */
//int getMoveCommand(char opcode, char* before, char* converted)
//{
//  char buff[10];
//  int  temp;
//  int  cont = 0;
//
//  converted[cont] = opcode;
//  ++cont;
//  cont = cont + convertHexToAscString(before, 4, converted + cont);
//  cont = cont + convertHexToAscString(before + 4, 4, converted + cont);
//  converted[cont] = '\0';
//
//#ifdef DEBUG
//  int i = 0;
//  for (i = 0; i < cont; i++) {
//    printf("%x\n", *(converted + i));
//  }
//#endif
//
//  return strlen(converted);
//}*/
//
////TBD:Create Utility Class. And move under method add static
//int convertDecToAsc(char* target, int length, int* converted) {
//  char buff[length + 1];
//  int  temp;
//
//  //strncpy(buff, before, length);
//  buff[length] = '\0';
//  //printf("ASC:%s\n", buff);
//
//  temp = atoi(buff);
//  //printf("DEC:%d\n", temp);
//
//  //converted = temp;
//
//  return 0;//strlen(converted);
//}

//
///*
// *
// */
//int convertHexToAscString(char* before, int length, char* converted)
//{
//  char buff[length + 1];
//  int  temp;
//  
//  printf("Param:%s\n", before);
//
//  strncpy(buff, before, length);
//  buff[length] = '\0';
//  printf("ASC:%s\n", buff);
//
//  temp = atoi(buff);
//  printf("DEC:%d\n", temp);
//
//  converted[0] = ((temp & 0xFF00) >> 8);
//  converted[1] = (temp & 0xFF);
//  converted[2] = '\0';
//  printf("HEX(MSB):%x\n", converted[0]);
//  printf("HEX(LSB):%x\n", converted[1]);
//  
//  return strlen(converted);
//}
//