Diff: main.cpp

Revision:

0:a037cf910cce

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 20 03:52:59 2017 +0900
@@ -0,0 +1,160 @@
+/* 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); */