kousuke miyaki / Mbed OS project_beta

Betaコントロール用のプログラム

Dependencies:   PS_PAD_re Servo

main.cpp@0:a037cf910cce, 2017-11-20 (annotated)

Committer:
Nyakkey from Desktop
Date:
Mon Nov 20 03:52:59 2017 +0900
Revision:
0:a037cf910cce
add first revision

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nyakkey from Desktop 0:a037cf910cce 1 /* Beta コントロール用のプログラム。
Nyakkey from Desktop 0:a037cf910cce 2 mbed-cli + github で開発してみるテストも兼ねているつもり。*/
Nyakkey from Desktop 0:a037cf910cce 3 #include "mbed.h"
Nyakkey from Desktop 0:a037cf910cce 4 #include "PS_PAD.h"
Nyakkey from Desktop 0:a037cf910cce 5 #include "Servo.h"
Nyakkey from Desktop 0:a037cf910cce 6
Nyakkey from Desktop 0:a037cf910cce 7
Nyakkey from Desktop 0:a037cf910cce 8 //inherit class
Nyakkey from Desktop 0:a037cf910cce 9 //PwmOut mortor_enable[] = {PC_0, PC_1, PC_2, PC_3} ;
Nyakkey from Desktop 0:a037cf910cce 10 //DigitalOut mortor_phase[] = {PC_4, PA_10, PB_4, PB_5} ;
Nyakkey from Desktop 0:a037cf910cce 11 PwmOut mortor_enable[] = {PC_1, PC_3, PC_2, PC_0} ;
Nyakkey from Desktop 0:a037cf910cce 12 DigitalOut mortor_phase[] = {PA_10, PB_5, PB_4, PC_4} ;
Nyakkey from Desktop 0:a037cf910cce 13 SPI ps_spi(PC_12, PC_11, PC_10) ;
Nyakkey from Desktop 0:a037cf910cce 14 PS_PAD ps2(&ps_spi, PD_2) ;
Nyakkey from Desktop 0:a037cf910cce 15 Servo servo[] = {PC_9, PC_6, PC_8, PA_6, PA_7, PC_7, PB_14, PB_15} ;
Nyakkey from Desktop 0:a037cf910cce 16 Serial pc (PA_2, PA_3) ;
Nyakkey from Desktop 0:a037cf910cce 17 //DigitalOut led (PA_5) ;
Nyakkey from Desktop 0:a037cf910cce 18
Nyakkey from Desktop 0:a037cf910cce 19 //global constant
Nyakkey from Desktop 0:a037cf910cce 20 const float SERVO_CENTER_VALUE[] = {
Nyakkey from Desktop 0:a037cf910cce 21 0.6,
Nyakkey from Desktop 0:a037cf910cce 22 0.65,
Nyakkey from Desktop 0:a037cf910cce 23 0.62,
Nyakkey from Desktop 0:a037cf910cce 24 0.6,
Nyakkey from Desktop 0:a037cf910cce 25 0.52,
Nyakkey from Desktop 0:a037cf910cce 26 0.53,
Nyakkey from Desktop 0:a037cf910cce 27 0.51,
Nyakkey from Desktop 0:a037cf910cce 28 0.54
Nyakkey from Desktop 0:a037cf910cce 29 } ;
Nyakkey from Desktop 0:a037cf910cce 30 const bool MORTOR_FORWARD[] = {1, 1, 1, 1} ;
Nyakkey from Desktop 0:a037cf910cce 31 const float MORTOR_MAXSPEED = 0.8 ;
Nyakkey from Desktop 0:a037cf910cce 32 const int PS2_ANALOG_RIGHT_OFFSET[2] = { 0, 1 } ;
Nyakkey from Desktop 0:a037cf910cce 33 //global varient
Nyakkey from Desktop 0:a037cf910cce 34
Nyakkey from Desktop 0:a037cf910cce 35 // other function
Nyakkey from Desktop 0:a037cf910cce 36 void init(){
Nyakkey from Desktop 0:a037cf910cce 37 int i = 0 ;
Nyakkey from Desktop 0:a037cf910cce 38 int return_from_ps2 = -1 ;
Nyakkey from Desktop 0:a037cf910cce 39
Nyakkey from Desktop 0:a037cf910cce 40 pc.baud(115200) ;
Nyakkey from Desktop 0:a037cf910cce 41 pc.printf("Reset\r\n") ;
Nyakkey from Desktop 0:a037cf910cce 42 for ( i = 0 ; i < 4 ; i++) {
Nyakkey from Desktop 0:a037cf910cce 43 mortor_enable[i].period(0.00001);
Nyakkey from Desktop 0:a037cf910cce 44 mortor_enable[i] = 0.0 ;
Nyakkey from Desktop 0:a037cf910cce 45 }
Nyakkey from Desktop 0:a037cf910cce 46 for ( i = 0 ; i < 8 ; i++ ) {
Nyakkey from Desktop 0:a037cf910cce 47 servo[i] = SERVO_CENTER_VALUE[i] ;
Nyakkey from Desktop 0:a037cf910cce 48 }
Nyakkey from Desktop 0:a037cf910cce 49 return_from_ps2 = ps2.init() ;
Nyakkey from Desktop 0:a037cf910cce 50 if ( return_from_ps2 == -1 ) {
Nyakkey from Desktop 0:a037cf910cce 51 pc.printf("PS2 Controller Fail to Start!\r\n") ;
Nyakkey from Desktop 0:a037cf910cce 52 } else if ( return_from_ps2 == 0 ) {
Nyakkey from Desktop 0:a037cf910cce 53 pc.printf("PS2 Controller Success to Start\r\n") ;
Nyakkey from Desktop 0:a037cf910cce 54 } else {
Nyakkey from Desktop 0:a037cf910cce 55 pc.printf("PS2 Controller return Unkown Error\r\n");
Nyakkey from Desktop 0:a037cf910cce 56 }
Nyakkey from Desktop 0:a037cf910cce 57 }
Nyakkey from Desktop 0:a037cf910cce 58
Nyakkey from Desktop 0:a037cf910cce 59 void mortor_ctrl(int _num, float _pow ){
Nyakkey from Desktop 0:a037cf910cce 60 //_num -> 0:frontside-right, 1:frontside-left, 2:backside-left, 3:backside-right,, _pow->duty(-1.0 to 1.0)
Nyakkey from Desktop 0:a037cf910cce 61 if ( _num < 0 || _num > 3 ) {
Nyakkey from Desktop 0:a037cf910cce 62 return ;
Nyakkey from Desktop 0:a037cf910cce 63 }
Nyakkey from Desktop 0:a037cf910cce 64 if ( _pow < 0 ) {
Nyakkey from Desktop 0:a037cf910cce 65 mortor_phase[_num] = ! MORTOR_FORWARD[_num] ;
Nyakkey from Desktop 0:a037cf910cce 66 mortor_enable[_num] = ( -1.f * _pow ) / 2.f + 0.5f ;
Nyakkey from Desktop 0:a037cf910cce 67 } else if ( _pow > 0 ) {
Nyakkey from Desktop 0:a037cf910cce 68 mortor_phase[_num] = MORTOR_FORWARD[_num] ;
Nyakkey from Desktop 0:a037cf910cce 69 mortor_enable[_num] = _pow / 2.f + 0.5f ;
Nyakkey from Desktop 0:a037cf910cce 70 } else {
Nyakkey from Desktop 0:a037cf910cce 71 mortor_enable[_num] = 0.0 ;
Nyakkey from Desktop 0:a037cf910cce 72 }
Nyakkey from Desktop 0:a037cf910cce 73 return ;
Nyakkey from Desktop 0:a037cf910cce 74 }
Nyakkey from Desktop 0:a037cf910cce 75
Nyakkey from Desktop 0:a037cf910cce 76 void move(float _x, float _y ){
Nyakkey from Desktop 0:a037cf910cce 77 float x[2] = { 0.0 , 0.0 };
Nyakkey from Desktop 0:a037cf910cce 78 x[0] = 0.7071 * ( _x + _y ) ;
Nyakkey from Desktop 0:a037cf910cce 79 x[1] = 0.7071 * ( _y - _x ) ;
Nyakkey from Desktop 0:a037cf910cce 80 for ( int i = 0 ; i < 2 ; i++ ) {
Nyakkey from Desktop 0:a037cf910cce 81 if ( x[i] > 0.99 ) {
Nyakkey from Desktop 0:a037cf910cce 82 x[i] = 1.0 ;
Nyakkey from Desktop 0:a037cf910cce 83 } else if ( x[i] < 0.01 && x[i] > -0.01 ) {
Nyakkey from Desktop 0:a037cf910cce 84 x[i] = 0.0 ;
Nyakkey from Desktop 0:a037cf910cce 85 } else if ( x[i] < -0.99 ) {
Nyakkey from Desktop 0:a037cf910cce 86 x[i] = -1.0 ;
Nyakkey from Desktop 0:a037cf910cce 87 }
Nyakkey from Desktop 0:a037cf910cce 88 x[i] = x[i] * MORTOR_MAXSPEED ;
Nyakkey from Desktop 0:a037cf910cce 89 }
Nyakkey from Desktop 0:a037cf910cce 90 pc.printf("MortorOutput, x0 = %f, x1 = %f\r\n", x[0], x[1] ) ;
Nyakkey from Desktop 0:a037cf910cce 91 mortor_ctrl(0, x[1]) ;
Nyakkey from Desktop 0:a037cf910cce 92 mortor_ctrl(1, x[0]) ;
Nyakkey from Desktop 0:a037cf910cce 93 mortor_ctrl(2, x[1]) ;
Nyakkey from Desktop 0:a037cf910cce 94 mortor_ctrl(3, x[0]) ;
Nyakkey from Desktop 0:a037cf910cce 95 return ;
Nyakkey from Desktop 0:a037cf910cce 96 }
Nyakkey from Desktop 0:a037cf910cce 97
Nyakkey from Desktop 0:a037cf910cce 98 // main function
Nyakkey from Desktop 0:a037cf910cce 99 int main() {
Nyakkey from Desktop 0:a037cf910cce 100 init() ;
Nyakkey from Desktop 0:a037cf910cce 101 while (true){
Nyakkey from Desktop 0:a037cf910cce 102 //run forever
Nyakkey from Desktop 0:a037cf910cce 103 int rx, ry ;
Nyakkey from Desktop 0:a037cf910cce 104 float x, y ;
Nyakkey from Desktop 0:a037cf910cce 105 ps2.poll() ;
Nyakkey from Desktop 0:a037cf910cce 106 rx = ps2.read(PS_PAD::ANALOG_RX) + PS2_ANALOG_RIGHT_OFFSET[0] ;
Nyakkey from Desktop 0:a037cf910cce 107 ry = ( ps2.read(PS_PAD::ANALOG_RY) + PS2_ANALOG_RIGHT_OFFSET[1] ) * -1.f;
Nyakkey from Desktop 0:a037cf910cce 108 pc.printf("Rx = %d, Ry = %d\r\n", rx, ry) ;
Nyakkey from Desktop 0:a037cf910cce 109 x = (float)rx / (127.f * 1.414) ;
Nyakkey from Desktop 0:a037cf910cce 110 y = (float)ry / (127.f * 1.414) ;
Nyakkey from Desktop 0:a037cf910cce 111 pc.printf("x = %f, y = %f\r\n", x, y) ;
Nyakkey from Desktop 0:a037cf910cce 112 move(x,y) ;
Nyakkey from Desktop 0:a037cf910cce 113 wait(0.1) ;
Nyakkey from Desktop 0:a037cf910cce 114 }
Nyakkey from Desktop 0:a037cf910cce 115 }
Nyakkey from Desktop 0:a037cf910cce 116
Nyakkey from Desktop 0:a037cf910cce 117
Nyakkey from Desktop 0:a037cf910cce 118 // TEST PROGRAM IN MAIN FUNCTION
Nyakkey from Desktop 0:a037cf910cce 119 /* motor test
Nyakkey from Desktop 0:a037cf910cce 120 int i = 0 ;
Nyakkey from Desktop 0:a037cf910cce 121 //pc.printf("debug\r\n") ;
Nyakkey from Desktop 0:a037cf910cce 122 for ( i = 0 ; i < 4 ; i++ ){
Nyakkey from Desktop 0:a037cf910cce 123 mortor_ctrl(i, 0.5) ;
Nyakkey from Desktop 0:a037cf910cce 124 }
Nyakkey from Desktop 0:a037cf910cce 125 wait(1.0) ;
Nyakkey from Desktop 0:a037cf910cce 126 for ( i = 0 ; i < 4 ; i++ ){
Nyakkey from Desktop 0:a037cf910cce 127 mortor_ctrl(i, 0.0) ;
Nyakkey from Desktop 0:a037cf910cce 128 }
Nyakkey from Desktop 0:a037cf910cce 129 wait(1.0) ; */
Nyakkey from Desktop 0:a037cf910cce 130 /* mortor test 2
Nyakkey from Desktop 0:a037cf910cce 131 for ( int i = 0 ; i < 4 ; i++ ) {
Nyakkey from Desktop 0:a037cf910cce 132 mortor_ctrl(i, 0.5) ;
Nyakkey from Desktop 0:a037cf910cce 133 wait(1.0) ;
Nyakkey from Desktop 0:a037cf910cce 134 mortor_ctrl(i, 0.0) ;
Nyakkey from Desktop 0:a037cf910cce 135 wait(1.0) ;
Nyakkey from Desktop 0:a037cf910cce 136 mortor_ctrl(i, -0.5) ;
Nyakkey from Desktop 0:a037cf910cce 137 wait(1.0) ;
Nyakkey from Desktop 0:a037cf910cce 138 mortor_ctrl(i, 0.0) ;
Nyakkey from Desktop 0:a037cf910cce 139 }
Nyakkey from Desktop 0:a037cf910cce 140 */
Nyakkey from Desktop 0:a037cf910cce 141 /* servo test
Nyakkey from Desktop 0:a037cf910cce 142 int i = 0 ;
Nyakkey from Desktop 0:a037cf910cce 143 for ( i = 0 ; i < 8 ; i++ ) {
Nyakkey from Desktop 0:a037cf910cce 144 servo[i] = SERVO_CENTER_VALUE[i] + 0.1 ;
Nyakkey from Desktop 0:a037cf910cce 145 wait(0.5) ;
Nyakkey from Desktop 0:a037cf910cce 146 servo[i] = SERVO_CENTER_VALUE[i] - 0.1 ;
Nyakkey from Desktop 0:a037cf910cce 147 wait(0.5) ;
Nyakkey from Desktop 0:a037cf910cce 148 }
Nyakkey from Desktop 0:a037cf910cce 149 wait(3.0) ; */
Nyakkey from Desktop 0:a037cf910cce 150
Nyakkey from Desktop 0:a037cf910cce 151 /* ps2 Controller test
Nyakkey from Desktop 0:a037cf910cce 152 float rx, ry ;
Nyakkey from Desktop 0:a037cf910cce 153 ps2.poll() ;
Nyakkey from Desktop 0:a037cf910cce 154 if (ps2.read(PS_PAD::PAD_CIRCLE) == 1) pc.printf("CIRCLE\n\r");
Nyakkey from Desktop 0:a037cf910cce 155 if (ps2.read(PS_PAD::PAD_SQUARE) == 1) pc.printf("SQUARE\n\r");
Nyakkey from Desktop 0:a037cf910cce 156 if (ps2.read(PS_PAD::PAD_TRIANGLE) == 1) pc.printf("TRIANGLE\n\r");
Nyakkey from Desktop 0:a037cf910cce 157 if (ps2.read(PS_PAD::PAD_X) == 1) pc.printf("X\n\r");
Nyakkey from Desktop 0:a037cf910cce 158 rx = ps2.read(PS_PAD::ANALOG_RX); ry = ps2.read(PS_PAD::ANALOG_RY);
Nyakkey from Desktop 0:a037cf910cce 159 pc.printf("Right X = %d, Y = %d\n\r", rx, ry);
Nyakkey from Desktop 0:a037cf910cce 160 wait(0.05); */