ハヤト タジリ
/
MAID_ROBOT_makerfair
スネーク
main.cpp
- Committer:
- tajiri1999
- Date:
- 2020-03-23
- Revision:
- 3:30d264c3da7d
- Parent:
- 2:e1fedc61e5e3
File content as of revision 3:30d264c3da7d:
#include "mbed.h" #include "Motor.h" //constant #define PI 3.1415926 //Skps Serial ps2(A0,A1,9600);//Tx,Rx(D8,D2) //SKPS_Button_number enum { //return value:digital p_select, p_joyl, p_joyr, p_start, p_up, p_right, p_down, p_left, p_l2, p_r2, p_l1, p_r1, p_triangle, p_circle, p_cross, p_square, //return_value:analog p_joy_lx, p_joy_ly, p_joy_rx, p_joy_ry, p_joy_lu, p_joy_ld, p_joy_ll, p_joy_lr, p_joy_ru, p_joy_rd, p_joy_rl, p_joy_rr, p_con_status, //return_value:none p_motor1, p_motor2 }; //TerminalDisplay(TeraTerm) Serial pc(SERIAL_TX, SERIAL_RX); //voltage AnalogIn voltage(PC_4); //UltraSonicSensor DigitalIn change(NC);//pa6 Timer timer_volt; //Motor Motor motor(PA_12,PA_11,PA_5, PB_2,PB_1,PA_9, PB_15,PB_14,PB_13, PA_8,PB_4,PB_10); //kicker DigitalOut kick(NC);//pa_8 //ToggleSwitch DigitalIn sw_start(PD_2); //program start switch DigitalIn sw_reset(PC_12); //gyro sensor reset switch DigitalIn sw_kick(USER_BUTTON); //declear prototype (function list) bool check_voltage(); //電圧をチェックする void uart_send(unsigned char data); unsigned char uart_rec(void); unsigned char skps(unsigned char data); unsigned char skps_vibrate(unsigned char motor, unsigned char value); float convert_degree(float x,float y); float convert_length(float x,float y); /*****************************************************************/ /**********************main function******************************/ /*****************************************************************/ int main() { //**************************************************************// ////////////////////////initialize setting//////////////////////// //**************************************************************// wait_ms(50); kick = 0; //キックを解除する /*motor pwm frequency set*/ motor.setPwmPeriod(0.00052); while (1) { //***************************************************************// ////////////////Play mode//////////// //***************************************************************// timer_volt.reset(); timer_volt.start(); while (sw_start == 0 && check_voltage() == 1) { float deg,power,yaw_angle,lx,ly,rx; //convert int to float lx = (float)skps(p_joy_lx); ly = (float)skps(p_joy_ly); rx = (float)skps(p_joy_rx); deg = convert_degree(lx,ly); yaw_angle = (rx - 128)/128; power = convert_length(lx,ly)*0.7; if(power <= 10) { //遊びをカット power = 0; } if(skps(p_l1) == 0 || skps(p_square) == 0){ motor.omniWheels(deg,power,yaw_angle*30); } else{ motor.omniWheels(deg,power*0.5,yaw_angle*30); } /*kick*/ if(skps(p_circle) == 0 || skps(p_r1)== 0){ kick = 1; wait_ms(100); kick = 0; } } motor.omniWheels(0, 0, 0); /**********************end main function**************************/ } } ////////////////////////////////////// /*Battery check voltage*/ ////////////////////////////////////// bool check_voltage() { static float sum[5] = { 8, 8, 8, 8, 8 }; static float Ave = 8; static bool S = 1; if (timer_volt.read() > 0.2) { for (int i = 4; i > 0; i--) { sum[i] = sum[i - 1]; } sum[0] = voltage.read() * 9.9; Ave = (sum[0] + sum[1] + sum[2] + sum[3] + sum[4]) / 5; if (Ave < 7.0) { //7.0V以下で自動遮断 S = 0; } else { S = 1; } timer_volt.reset(); } return S; } void uart_send(unsigned char data) { ps2.putc(data); } unsigned char uart_rec(void) { unsigned char temp; while(ps2.readable() == 0); temp = ps2.getc(); return temp; } unsigned char skps(unsigned char data) { uart_send(data); return uart_rec(); } unsigned char skps_vibrate(unsigned char motor, unsigned char value) { uart_send(motor); uart_send(value); } /**************************************************************/ /*関数名:convert_degree(float x,float y)*/ /*引数:ジョイスティックのX,Y Axis*/ /*戻り値:float型 極座標の角度(前方が0度,時計回りに360度)*/ /*概要:ロボットの進行方向を決めるときに使う*/ /*************************************************************/ float convert_degree(float x,float y){ float degree; degree = (180/PI)*atan2(y - 128,x - 128) + 360 + 90;//オイラー角度に変換 //角度を0~360度に変換 if(degree >= 360.0){ degree = degree - 360; } return degree; } /***************************************************************/ /*関数名:convert_length(float x,flaot y)*/ /*引数:ジョイスティックのX,Y*/ /*戻り値:float型 極座標の原点から(x,y)点までの長さを0~100で返す*/ /*概要:モーターのパワーを決めるときに,極座標の長さを使う*/ /***************************************************************/ float convert_length(float x,float y){ float xy_length; xy_length = pow(x - 128,2) + pow(y - 128,2); xy_length = (100 * xy_length)/(128*128); if(xy_length >= 100.0){ xy_length = 100.0; } return xy_length; }