File content as of revision 8:857b3285dc79:

#include <mbed.h>
#include <Serial.h>
#include "def_resources.h"
#include "PwmServo.h"
#include "ExtendedServo.h"
#include "DelayServo.h"
#include "Roomba.h"
#include "DerivedRoomba.h"

/*
    Controller firmware for CaitSithDanger

    Kyoto-Densou-An 2014

    Author : yishii
*/

#define DEBUG
#define BUFFER_SIZE 15

int oyatsuCnt = 0;

bool initialize();
int  serial_gets(char *buffer, int size);
void control_caitsith(char* buffer, int size);
void switchNyandaBeam(bool isFiring);
void controlOyatsuFeeder(bool isMoving);

SendData::SendData data;

//Ticker mTick;
//    Roomba* cp = new Roomba(SERIAL_TX, SERIAL_RX, LED1);//Class pointor
//    mTick.attach(cp, &Roomba::periodicCallback, 0.1);
  

/*
 *
 */

    int leftWheelVelocity = 0;
    int rightWheelVelocity = 0;

int main() {
    pc.baud(115200);
    initialize();

    //SendData::SendData *data = new SendData(0, 0);
    //base.set(*data);

    int timecount = 0;
    while(1) {
        char buffer[BUFFER_SIZE];
        int size = serial_gets(buffer, BUFFER_SIZE);
        if (size > 0) {
            control_caitsith(buffer, size);
        }
        if (timecount >= 10000) {
            base.periodicCallback();//ルンバへ移動指示
            timecount = 0;
        }
        timecount++;
        wait_us(10);
    }
}


//bool
void control_caitsith(char* buffer, int size) {
#ifdef DEBUG
    pc.printf("%s", buffer);
#endif
    // コマンド判定
    if (strncmp(buffer, "CT", 2) == 0) {
        bool ret = false;
        
        myled = !myled;
        
        if (size == 2) {
            ret = true;
        
        } else if (size >= 3) {
            switch (buffer[2]) {
                //緊急停止
                case 'Z':
                    initialize();
                    ret = true;
                    break;
                //ニャンだろー光線
                case 'N':
                    bool isFiring = ( buffer[3] == '1' ) ? true : false;
                    switchNyandaBeam(isFiring);
                    ret = true;//isFiring;
                    break;
                //おやつベンダ
                case 'O':
                    bool isMoving = ( buffer[3] == '1' ) ? true : false;
                    controlOyatsuFeeder(isMoving);
                    ret = true;//isMoving;
                    break;
                //ルンバ移動指示
                case 'M'://CTM+500-500
                    ret = true;
                    
                    int left = 0;
                    int right = 0;
                    
                    int length = strlen(buffer);
                    pc.printf("length:%d\n", length);
                    if(buffer[3] == 'L') {
                        left = atoi(buffer + 4);
                        pc.printf("left:%d\n", left);

                        int i = 4;
                        while (i < length) {
                            i++;
                            if (buffer[i] == 'R') {
                                pc.printf("find R index:%d\n", i);
                                break;
                            }
                        }
                        int right = atoi(buffer + i+1);
                        pc.printf("right:%d\n", right);

                    } else {
                        ret = false;
                        break;
                    }
                    
//
//                    char buff[4+1];
//                    int  temp;
//                    int  leftWheelVelocity, rightWheelVelocity;
//  
//                    while (buffer + i == 'R') {
//                        i++;
//                    }
//  
//                    //pc.printf("Param:%s\n", before);
//                    strncpy(buff, buffer + 4, 4);
//                    buff[4] = '\0';
//                    pc.printf("%s\n", buff);
//                    leftWheelVelocity = atoi(buff);
//                    pc. printf("%d\n", leftWheelVelocity);                
//                
//                    //printf("Param:%s\n", before);
//                    strncpy(buff, buffer + 9, 4);//(char*)buffer[3+4]
//                    buff[4] = '\0';
//                    pc. printf("%s\n", buff);
//                    rightWheelVelocity = atoi(buff);
//                    pc. printf("%d\n", rightWheelVelocity);
            
                    //SendData::SendData *data = new SendData(leftWheelVelocity, rightWheelVelocity);
                    
                    data.wheelVelocity[0] = left;//leftWheelVelocity;
                    data.wheelVelocity[1] = right;//rightWheelVelocity;

                    pc. printf("%d, %d\n", data.wheelVelocity[0], data.wheelVelocity[1]);

                    
                    base.set(&data);

                    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;
            }            
        } else {
            ret = false;    
        }

        //処理結果出力
        if (ret) {
            pc.printf("\r\nOK\r\n");
        } else {
            pc.printf("\r\nNG\r\n");
        }
    }
}


//bool control_joint(char *buffer, ExtendedServo *joint1, ExtendedServo *joint2)
//{
//    bool ret = true;
//    int servoAngle = -1;
//    
//    servoAngle = atoi(buffer);
//            
//    ret = true;
//    if (buffer[3] == '1') {
//        joint1->setAngle(servoAngle);
//    } else if (buffer[3] == '2') {
//        joint2->setAngle(servoAngle);
//    } else {
//        ret = false;
//    }
//  
//    return ret;
//}


/*
 *
 */
bool initialize() {
    //pc.printf("init");
    
    nyandaBeam = 0;
  
    shakeHead.setAngle(0);
    nodHead.setAngle(0);
    leftArm.setAngle(0);
    leftShoulder.setAngle(0);
    rightArm.setAngle(0);
    rightShoulder.setAngle(0);
    
    //Roomba
    SendData::SendData *data = new SendData(0, 0);
    base.set(data);
    delete data;
    
    return true;
}


/*
 *
 */
void switchNyandaBeam(bool isFiring) {
    //pc.printf("beam");
    
    if (isFiring) {
        nyandaBeam = 1;
    } else {
        nyandaBeam = 0;  
    }
}


//給仕機を操作する
void controlOyatsuFeeder(bool isMoving) {
    //pc.printf("feeder");
    
    int feederDegree = 0;
    
    if (isMoving) {
        if (oyatsuCnt < 3) {
            oyatsuCnt++;
        }
        //60, 90, 120
        feederDegree = 60 + 30 * (oyatsuCnt - 1);
    } else {
        oyatsuCnt = 0;
    }
    
    oyatsuFeeder.setAngle(feederDegree);
}



/*
 * 
 */
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;
        break;
      }
      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);
//}
//







//    switch (buffer[2]) {
//                case 'Z':
//                    ret = initialize();
//                    break;
//        
//                case 'N':
//                    bool isFiring = ( buffer[3] == '1' ) ? true : false;
//                    switchNyandaBeam(isFiring);
//                    ret = isFiring;
//                    break;
//            
//                case 'M'://CTM+500-500
//                ret = true;
//            
//                char buff[4+1];
//                int  temp;
//  
//                //pc.printf("Param:%s\n", before);
//                strncpy(buff, buffer + 3, 4);
//                buff[4] = '\0';
//                //pc.printf("ASC:%s\n", buff);
//                temp = atoi(buff);
//                e.leftWheetVelocity = temp;            
//                //printf("Param:%s\n", before);
//                strncpy(buff, buffer + 7, 4);//(char*)buffer[3+4]
//                buff[4] = '\0';
//                //pc. printf("ASC:%s\n", buff);
//                temp = atoi(buff);
//                e.rightWheelVelocity = temp;
//            
//                break;
//              
//                case 'O':
//                ret = true;
//            
//                if (buffer[3] == '0') {
//                    oyatsuCnt = 0;
//                } else if (buffer[3] == '1') {
//                    if (oyatsuCnt < 3) {
//                    oyatsuCnt++;
//                }
//                
//                switch (oyatsuCnt) {
//                    case 0:
//                        oyatsuFeeder.setAngle(0);
//                    break;
//                    
//                    case 1:
//                        oyatsuFeeder.setAngle(60);
//                    break;
//                
//                    case 2:
//                        oyatsuFeeder.setAngle(90);
//                    break;
//                
//                    case 3:
//                        oyatsuFeeder.setAngle(120);
//                    break;
//                
//                    default:
//                        //oyatsuFeeder.setAngle(0);
//                    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;
//            }
//        }