main.cpp

00001 /* Beta コントロール用のプログラム。
00002 mbed-cli + github で開発してみるテストも兼ねているつもり。*/
00003 #include "mbed.h"
00004 #include "PS_PAD.h"
00005 #include "Servo.h"
00006 
00007 
00008 //inherit class
00009 //PwmOut mortor_enable[] = {PC_0, PC_1, PC_2, PC_3} ;
00010 //DigitalOut mortor_phase[] = {PC_4, PA_10, PB_4, PB_5} ;
00011 PwmOut mortor_enable[] = {PC_1, PC_3, PC_2, PC_0} ;
00012 DigitalOut mortor_phase[] = {PA_10, PB_5, PB_4, PC_4} ;
00013 SPI ps_spi(PC_12, PC_11, PC_10) ;
00014 PS_PAD ps2(&ps_spi, PD_2) ;
00015 Servo servo[] = {PC_9, PC_6, PC_8, PA_6, PA_7, PC_7, PB_14, PB_15} ;
00016 Serial pc (PA_2, PA_3) ;
00017 //DigitalOut led (PA_5) ;
00018 
00019 //global constant
00020 const float SERVO_CENTER_VALUE[] = {
00021   0.6,
00022   0.65,
00023   0.62,
00024   0.6,
00025   0.52,
00026   0.53,
00027   0.51,
00028   0.54
00029 } ;
00030 const bool MORTOR_FORWARD[] = {1, 1, 1, 1} ;
00031 const float MORTOR_MAXSPEED = 0.8 ;
00032 const int PS2_ANALOG_RIGHT_OFFSET[2] = { 0, 1 } ;
00033 //global varient
00034 
00035 // other function
00036 void init(){
00037   int i = 0 ;
00038   int return_from_ps2 = -1 ;
00039 
00040   pc.baud(115200) ;
00041   pc.printf("Reset\r\n") ;
00042   for ( i = 0 ; i < 4 ; i++) {
00043     mortor_enable[i].period(0.00001);
00044     mortor_enable[i] = 0.0 ;
00045   }
00046   for ( i = 0 ; i < 8 ; i++ ) {
00047     servo[i] = SERVO_CENTER_VALUE[i] ;
00048   }
00049   return_from_ps2 = ps2.init() ;
00050   if ( return_from_ps2 == -1 ) {
00051     pc.printf("PS2 Controller Fail to Start!\r\n") ;
00052   } else if ( return_from_ps2 == 0 ) {
00053     pc.printf("PS2 Controller Success to Start\r\n") ;
00054   } else {
00055     pc.printf("PS2 Controller return Unkown Error\r\n");
00056   }
00057 }
00058 
00059 void mortor_ctrl(int _num, float _pow ){
00060   //_num -> 0:frontside-right, 1:frontside-left, 2:backside-left, 3:backside-right,,  _pow->duty(-1.0 to 1.0)
00061   if ( _num < 0 || _num > 3 ) {
00062     return ;
00063   }
00064   if ( _pow < 0 ) {
00065     mortor_phase[_num] = ! MORTOR_FORWARD[_num] ;
00066     mortor_enable[_num] = ( -1.f * _pow ) / 2.f + 0.5f ;
00067   } else if ( _pow > 0 ) {
00068     mortor_phase[_num] = MORTOR_FORWARD[_num] ;
00069     mortor_enable[_num] = _pow / 2.f + 0.5f ;
00070   } else {
00071     mortor_enable[_num] = 0.0 ;
00072   }
00073   return ;
00074 }
00075 
00076 void move(float _x, float _y ){
00077   float x[2] = { 0.0 , 0.0 };
00078   x[0] = 0.7071 * ( _x + _y ) ;
00079   x[1] = 0.7071 * ( _y - _x ) ;
00080   for ( int i = 0 ; i < 2 ; i++ ) {
00081     if ( x[i] > 0.99 ) {
00082       x[i] = 1.0 ;
00083     } else if ( x[i] < 0.01 && x[i] > -0.01 ) {
00084       x[i] = 0.0 ;
00085     } else if ( x[i] < -0.99 ) {
00086       x[i] = -1.0 ;
00087     }
00088     x[i] = x[i] * MORTOR_MAXSPEED ;
00089   }
00090   pc.printf("MortorOutput, x0 = %f, x1 = %f\r\n", x[0], x[1] ) ;
00091   mortor_ctrl(0, x[1]) ;
00092   mortor_ctrl(1, x[0]) ;
00093   mortor_ctrl(2, x[1]) ;
00094   mortor_ctrl(3, x[0]) ;
00095   return ;
00096 }
00097 
00098 // main function
00099 int main() {
00100   init() ;
00101   while (true){
00102     //run forever
00103     int rx, ry ;
00104     float x, y ;
00105     ps2.poll() ;
00106     rx = ps2.read(PS_PAD::ANALOG_RX) + PS2_ANALOG_RIGHT_OFFSET[0] ;
00107     ry = ( ps2.read(PS_PAD::ANALOG_RY) + PS2_ANALOG_RIGHT_OFFSET[1] ) * -1.f;
00108     pc.printf("Rx = %d, Ry = %d\r\n", rx, ry) ;
00109     x = (float)rx / (127.f * 1.414) ;
00110     y = (float)ry / (127.f * 1.414) ;
00111     pc.printf("x = %f, y = %f\r\n", x, y) ;
00112     move(x,y) ;
00113     wait(0.1) ;
00114   }
00115 }
00116 
00117 
00118 // TEST PROGRAM IN MAIN FUNCTION
00119 /*  motor test
00120 int i = 0 ;
00121 //pc.printf("debug\r\n") ;
00122 for ( i = 0 ; i < 4 ; i++ ){
00123   mortor_ctrl(i, 0.5) ;
00124 }
00125 wait(1.0) ;
00126 for ( i = 0 ; i < 4 ; i++ ){
00127   mortor_ctrl(i, 0.0) ;
00128 }
00129 wait(1.0) ; */
00130 /*  mortor test 2
00131 for ( int i = 0 ; i < 4 ; i++ ) {
00132   mortor_ctrl(i, 0.5) ;
00133   wait(1.0) ;
00134   mortor_ctrl(i, 0.0) ;
00135   wait(1.0) ;
00136   mortor_ctrl(i, -0.5) ;
00137   wait(1.0) ;
00138   mortor_ctrl(i, 0.0) ;
00139 }
00140 */
00141 /*  servo test
00142 int i = 0 ;
00143 for ( i = 0 ; i < 8 ; i++ ) {
00144   servo[i] = SERVO_CENTER_VALUE[i] + 0.1 ;
00145   wait(0.5) ;
00146   servo[i] = SERVO_CENTER_VALUE[i] - 0.1 ;
00147   wait(0.5) ;
00148 }
00149 wait(3.0) ; */
00150 
00151 /*  ps2 Controller test
00152 float rx, ry ;
00153 ps2.poll() ;
00154 if (ps2.read(PS_PAD::PAD_CIRCLE) == 1) pc.printf("CIRCLE\n\r");
00155 if (ps2.read(PS_PAD::PAD_SQUARE) == 1) pc.printf("SQUARE\n\r");
00156 if (ps2.read(PS_PAD::PAD_TRIANGLE) == 1) pc.printf("TRIANGLE\n\r");
00157 if (ps2.read(PS_PAD::PAD_X) == 1) pc.printf("X\n\r");
00158 rx = ps2.read(PS_PAD::ANALOG_RX); ry = ps2.read(PS_PAD::ANALOG_RY);
00159 pc.printf("Right X = %d, Y = %d\n\r", rx, ry);
00160 wait(0.05); */