File content as of revision 0:a037cf910cce:

/* Beta コントロール用のプログラム。
mbed-cli + github で開発してみるテストも兼ねているつもり。*/
#include "mbed.h"
#include "PS_PAD.h"
#include "Servo.h"


//inherit class
//PwmOut mortor_enable[] = {PC_0, PC_1, PC_2, PC_3} ;
//DigitalOut mortor_phase[] = {PC_4, PA_10, PB_4, PB_5} ;
PwmOut mortor_enable[] = {PC_1, PC_3, PC_2, PC_0} ;
DigitalOut mortor_phase[] = {PA_10, PB_5, PB_4, PC_4} ;
SPI ps_spi(PC_12, PC_11, PC_10) ;
PS_PAD ps2(&ps_spi, PD_2) ;
Servo servo[] = {PC_9, PC_6, PC_8, PA_6, PA_7, PC_7, PB_14, PB_15} ;
Serial pc (PA_2, PA_3) ;
//DigitalOut led (PA_5) ;

//global constant
const float SERVO_CENTER_VALUE[] = {
  0.6,
  0.65,
  0.62,
  0.6,
  0.52,
  0.53,
  0.51,
  0.54
} ;
const bool MORTOR_FORWARD[] = {1, 1, 1, 1} ;
const float MORTOR_MAXSPEED = 0.8 ;
const int PS2_ANALOG_RIGHT_OFFSET[2] = { 0, 1 } ;
//global varient

// other function
void init(){
  int i = 0 ;
  int return_from_ps2 = -1 ;

  pc.baud(115200) ;
  pc.printf("Reset\r\n") ;
  for ( i = 0 ; i < 4 ; i++) {
    mortor_enable[i].period(0.00001);
    mortor_enable[i] = 0.0 ;
  }
  for ( i = 0 ; i < 8 ; i++ ) {
    servo[i] = SERVO_CENTER_VALUE[i] ;
  }
  return_from_ps2 = ps2.init() ;
  if ( return_from_ps2 == -1 ) {
    pc.printf("PS2 Controller Fail to Start!\r\n") ;
  } else if ( return_from_ps2 == 0 ) {
    pc.printf("PS2 Controller Success to Start\r\n") ;
  } else {
    pc.printf("PS2 Controller return Unkown Error\r\n");
  }
}

void mortor_ctrl(int _num, float _pow ){
  //_num -> 0:frontside-right, 1:frontside-left, 2:backside-left, 3:backside-right,,  _pow->duty(-1.0 to 1.0)
  if ( _num < 0 || _num > 3 ) {
    return ;
  }
  if ( _pow < 0 ) {
    mortor_phase[_num] = ! MORTOR_FORWARD[_num] ;
    mortor_enable[_num] = ( -1.f * _pow ) / 2.f + 0.5f ;
  } else if ( _pow > 0 ) {
    mortor_phase[_num] = MORTOR_FORWARD[_num] ;
    mortor_enable[_num] = _pow / 2.f + 0.5f ;
  } else {
    mortor_enable[_num] = 0.0 ;
  }
  return ;
}

void move(float _x, float _y ){
  float x[2] = { 0.0 , 0.0 };
  x[0] = 0.7071 * ( _x + _y ) ;
  x[1] = 0.7071 * ( _y - _x ) ;
  for ( int i = 0 ; i < 2 ; i++ ) {
    if ( x[i] > 0.99 ) {
      x[i] = 1.0 ;
    } else if ( x[i] < 0.01 && x[i] > -0.01 ) {
      x[i] = 0.0 ;
    } else if ( x[i] < -0.99 ) {
      x[i] = -1.0 ;
    }
    x[i] = x[i] * MORTOR_MAXSPEED ;
  }
  pc.printf("MortorOutput, x0 = %f, x1 = %f\r\n", x[0], x[1] ) ;
  mortor_ctrl(0, x[1]) ;
  mortor_ctrl(1, x[0]) ;
  mortor_ctrl(2, x[1]) ;
  mortor_ctrl(3, x[0]) ;
  return ;
}

// main function
int main() {
  init() ;
  while (true){
    //run forever
    int rx, ry ;
    float x, y ;
    ps2.poll() ;
    rx = ps2.read(PS_PAD::ANALOG_RX) + PS2_ANALOG_RIGHT_OFFSET[0] ;
    ry = ( ps2.read(PS_PAD::ANALOG_RY) + PS2_ANALOG_RIGHT_OFFSET[1] ) * -1.f;
    pc.printf("Rx = %d, Ry = %d\r\n", rx, ry) ;
    x = (float)rx / (127.f * 1.414) ;
    y = (float)ry / (127.f * 1.414) ;
    pc.printf("x = %f, y = %f\r\n", x, y) ;
    move(x,y) ;
    wait(0.1) ;
  }
}


// TEST PROGRAM IN MAIN FUNCTION
/*  motor test
int i = 0 ;
//pc.printf("debug\r\n") ;
for ( i = 0 ; i < 4 ; i++ ){
  mortor_ctrl(i, 0.5) ;
}
wait(1.0) ;
for ( i = 0 ; i < 4 ; i++ ){
  mortor_ctrl(i, 0.0) ;
}
wait(1.0) ; */
/*  mortor test 2
for ( int i = 0 ; i < 4 ; i++ ) {
  mortor_ctrl(i, 0.5) ;
  wait(1.0) ;
  mortor_ctrl(i, 0.0) ;
  wait(1.0) ;
  mortor_ctrl(i, -0.5) ;
  wait(1.0) ;
  mortor_ctrl(i, 0.0) ;
}
*/
/*  servo test
int i = 0 ;
for ( i = 0 ; i < 8 ; i++ ) {
  servo[i] = SERVO_CENTER_VALUE[i] + 0.1 ;
  wait(0.5) ;
  servo[i] = SERVO_CENTER_VALUE[i] - 0.1 ;
  wait(0.5) ;
}
wait(3.0) ; */

/*  ps2 Controller test
float rx, ry ;
ps2.poll() ;
if (ps2.read(PS_PAD::PAD_CIRCLE) == 1) pc.printf("CIRCLE\n\r");
if (ps2.read(PS_PAD::PAD_SQUARE) == 1) pc.printf("SQUARE\n\r");
if (ps2.read(PS_PAD::PAD_TRIANGLE) == 1) pc.printf("TRIANGLE\n\r");
if (ps2.read(PS_PAD::PAD_X) == 1) pc.printf("X\n\r");
rx = ps2.read(PS_PAD::ANALOG_RX); ry = ps2.read(PS_PAD::ANALOG_RY);
pc.printf("Right X = %d, Y = %d\n\r", rx, ry);
wait(0.05); */