File content as of revision 0:a9b204e27472:

#include <mbed.h>
#include <PwmServo.h>
#include <RobotArm.h>
#include <def_resources.h>
#include <Serial.h>

/*

    Controller firmware for CaitSithDanger

    Kyoto-Densou-An 2014

    Author : yishii
*/

#define DEBUG
#define BUFFER_SIZE 11
#define COMMAND_TOTAL 3

Serial pc(SERIAL_TX, SERIAL_RX);
 
DigitalOut myled(LED1);

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

int head0;
int head1;

void pc_rx ();

int serial_gets (char *buffer, int size);

int main() {
  myled = 1;
  
  head0 = 90;
  head0 = head0 - 10;
  head1 = 90;
  head1 = head1 - 20;
  arm1.actuate(head0, head1);
  
  while(1) {
    char buffer[BUFFER_SIZE];
    
    int size = serial_gets(buffer, BUFFER_SIZE);
    if (size > 0) {
      pc.printf("%s", buffer);
      
      if (strncmp(buffer, "CT", 2) == 0) {
        
        myled = !myled;
        
        if (size == 2) {
          pc.printf("\r\nOK\r\n");
        } else if (size >= 3) {
          switch (buffer[2]) {
          case 'O':
          
            break;
          case 'H':
          
            int servoAngle = -1;
            servoAngle = atoi((buffer+4));
            //printf("%c\n", buffer[3]);
            //printf("%d\n", servoAngle);
            
            servoAngle += 90;
            servoAngle = 180 - servoAngle;
                        
            if (servoAngle < 20) {
              servoAngle = 20;
            }
            if (servoAngle > 160) {
              servoAngle = 160;
            }
                        

            if (buffer[3] == '1') {
              head0 = servoAngle;
              head0 = head0 - 10;
              //printf("%d\n", head0);
            } else if (buffer[3] == '2') {
              head1 = servoAngle;
              head1 = head1 - 20;
              //printf("%d\n", head1);
            } else {
                
            }

            arm1.actuate(head0, head1);

            break;
          }
          
          pc.printf("\r\nOK\r\n");
        }
        
      }
      
    }
    
  }
}



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 main()
//{
//  head0 = 90;
//  head0 = head0 - 10;
//  head1 = 90;
//  head1 = head1 - 20;

  //myled = 0;
  
  //clear buffer
//  memset(receive, 0, BUFFER_SIZE);
  //receive[BUFFER_SIZE-1] = '\0';
  //p_receive = receive;

  //TODO:Create arm instance.
  
  
//  arm1.actuate(head0, head1);
  
  
  //pc.attach(pc_rx, Serial::RxIrq);
  
//  while(1) {
//  }    

//    int i;
//
//    while(1) {
//        for(i=0; i<180; i++) {
//            arm1.actuate(i,i);
//            wait(0.005);
//        }
//        for(i=180; i>0; i--) {
//            arm1.actuate(i,i);
//            wait(0.005);
//        }
//    }
//}

void pc_rx () {
  int result = -1;
  
  char c = pc.getc();
  //pc.printf("%02X", (unsigned char)c);
  pc.printf("%c", (unsigned char)c);
  
  if (c == '\r') {
    //Nothign to do.
  } else if (c == '\n') {
    receive[recvPnt] = '\0';
    ++recvPnt;    
  } else {
    receive[recvPnt] = c;
    ++recvPnt;
  }

  
  if (c == '\n') {
    if (receive[0] == 'C' && receive[1] == 'T') {
      
      myled = 1 - myled;
      
      if (recvPnt == 3) {
        pc.printf("\r\nOK\r\n");
        
      } else if (recvPnt >= 4) {
        switch (receive[2]) {
          case 'O':
          
            break;
          
          case 'H':
            //myled = 1;            
            int servoAngle = -1;
            servoAngle = atoi((receive+4));
            
            servoAngle += 90;
            servoAngle = 180 - servoAngle;
                        
            if (servoAngle < 20) {
              servoAngle = 20;
            }
            if (servoAngle > 160) {
              servoAngle = 160;
            }
                        
            //printf("%d\n", servoAngle);

            int servoNo = -1;
            
            if (receive[3] == '1') {
              servoNo = 0;
              head0 = servoAngle;
              head0 = head0 - 10;
            } else if (receive[3] == '2') {
              servoNo = 1;
              head1 = servoAngle;
              head1 = head1 - 20;
            } else {
              servoNo = -1;
            }
            //printf("%d\n", servoNo);

            //arm1.actuate(servoNo, servoAngle);
            arm1.actuate(head0, head1);

            result = 0;
            pc.printf("\r\nOK\r\n");
            
            break;
          
          case 'L':
            pc.printf("\r\nOK\r\n");
            break;

          case 'R':
            pc.printf("\r\nOK\r\n");
            break;
          
          case 'N':
          
            if (receive[3] == '1') {
              myled = 1;
            } else {
              myled = 0;
            }
          
            pc.printf("\r\nOK\r\n");
            break;
          
          case 'M':
            pc.printf("\r\nOK\r\n");
            break;

          case 'Z':
            pc.printf("\r\nOK\r\n");
            break;
          
          default:
          
            result = 1;
            pc.printf("\r\nERROR\r\n");
            break;
        }
      } else {
        result = 1;
        pc.printf("\r\nERROR\r\n");
      }
    }
    
    //TODO:Send result the following code: CT[number]\r\n
    
    memset(receive, 0, BUFFER_SIZE);
    recvPnt = 0;
  }
  
  if (recvPnt >= BUFFER_SIZE) {
    //myled = 0;
    //pc.printf("NG1:Reset receive buffer.\n");
    memset(receive, 0, BUFFER_SIZE);
    recvPnt = 0;
  }
  
}