ハヤト タジリ / Mbed 2 deprecated MAID_ROBOT_makerfair

スネーク

Dependencies:   mbed MAIDROBO

main.cpp@3:30d264c3da7d, 2020-03-23 (annotated)

Committer:
tajiri1999
Date:
Mon Mar 23 13:45:57 2020 +0000
Revision:
3:30d264c3da7d
Parent:
2:e1fedc61e5e3 
`

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tajiri1999 0:6c2e6a485519 1 #include "mbed.h"
tajiri1999 0:6c2e6a485519 2 #include "Motor.h"
tajiri1999 0:6c2e6a485519 3
tajiri1999 1:e49ca9ffcccb 4 //constant
tajiri1999 1:e49ca9ffcccb 5 #define PI 3.1415926
tajiri1999 0:6c2e6a485519 6
tajiri1999 1:e49ca9ffcccb 7 //Skps
tajiri1999 1:e49ca9ffcccb 8 Serial ps2(A0,A1,9600);//Tx,Rx(D8,D2)
tajiri1999 0:6c2e6a485519 9
tajiri1999 1:e49ca9ffcccb 10 //SKPS_Button_number
tajiri1999 1:e49ca9ffcccb 11 enum {
tajiri1999 1:e49ca9ffcccb 12 //return value：digital
tajiri1999 1:e49ca9ffcccb 13 p_select,
tajiri1999 1:e49ca9ffcccb 14 p_joyl,
tajiri1999 1:e49ca9ffcccb 15 p_joyr,
tajiri1999 1:e49ca9ffcccb 16 p_start,
tajiri1999 1:e49ca9ffcccb 17 p_up,
tajiri1999 1:e49ca9ffcccb 18 p_right,
tajiri1999 1:e49ca9ffcccb 19 p_down,
tajiri1999 1:e49ca9ffcccb 20 p_left,
tajiri1999 1:e49ca9ffcccb 21 p_l2,
tajiri1999 1:e49ca9ffcccb 22 p_r2,
tajiri1999 1:e49ca9ffcccb 23 p_l1,
tajiri1999 1:e49ca9ffcccb 24 p_r1,
tajiri1999 1:e49ca9ffcccb 25 p_triangle,
tajiri1999 1:e49ca9ffcccb 26 p_circle,
tajiri1999 1:e49ca9ffcccb 27 p_cross,
tajiri1999 1:e49ca9ffcccb 28 p_square,
tajiri1999 1:e49ca9ffcccb 29 //return_value：analog
tajiri1999 1:e49ca9ffcccb 30 p_joy_lx,
tajiri1999 1:e49ca9ffcccb 31 p_joy_ly,
tajiri1999 1:e49ca9ffcccb 32 p_joy_rx,
tajiri1999 1:e49ca9ffcccb 33 p_joy_ry,
tajiri1999 1:e49ca9ffcccb 34 p_joy_lu,
tajiri1999 1:e49ca9ffcccb 35 p_joy_ld,
tajiri1999 1:e49ca9ffcccb 36 p_joy_ll,
tajiri1999 1:e49ca9ffcccb 37 p_joy_lr,
tajiri1999 1:e49ca9ffcccb 38 p_joy_ru,
tajiri1999 1:e49ca9ffcccb 39 p_joy_rd,
tajiri1999 1:e49ca9ffcccb 40 p_joy_rl,
tajiri1999 1:e49ca9ffcccb 41 p_joy_rr,
tajiri1999 1:e49ca9ffcccb 42 p_con_status,
tajiri1999 0:6c2e6a485519 43
tajiri1999 1:e49ca9ffcccb 44 //return_value：none
tajiri1999 1:e49ca9ffcccb 45 p_motor1,
tajiri1999 1:e49ca9ffcccb 46 p_motor2
tajiri1999 1:e49ca9ffcccb 47 };
tajiri1999 0:6c2e6a485519 48
tajiri1999 1:e49ca9ffcccb 49 //TerminalDisplay(TeraTerm)
tajiri1999 1:e49ca9ffcccb 50 Serial pc(SERIAL_TX, SERIAL_RX);
tajiri1999 0:6c2e6a485519 51
tajiri1999 1:e49ca9ffcccb 52 //voltage
tajiri1999 1:e49ca9ffcccb 53 AnalogIn voltage(PC_4);
tajiri1999 0:6c2e6a485519 54
tajiri1999 1:e49ca9ffcccb 55 //UltraSonicSensor
tajiri1999 1:e49ca9ffcccb 56 DigitalIn change(NC);//pa6
tajiri1999 1:e49ca9ffcccb 57 Timer timer_volt;
tajiri1999 0:6c2e6a485519 58
tajiri1999 1:e49ca9ffcccb 59 //Motor
tajiri1999 1:e49ca9ffcccb 60 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);
tajiri1999 0:6c2e6a485519 61
tajiri1999 1:e49ca9ffcccb 62 //kicker
tajiri1999 1:e49ca9ffcccb 63 DigitalOut kick(NC);//pa_8
tajiri1999 0:6c2e6a485519 64
tajiri1999 1:e49ca9ffcccb 65 //ToggleSwitch
tajiri1999 1:e49ca9ffcccb 66 DigitalIn sw_start(PD_2); //program start switch
tajiri1999 1:e49ca9ffcccb 67 DigitalIn sw_reset(PC_12); //gyro sensor reset switch
tajiri1999 1:e49ca9ffcccb 68 DigitalIn sw_kick(USER_BUTTON);
tajiri1999 0:6c2e6a485519 69
tajiri1999 1:e49ca9ffcccb 70 //declear prototype (function list)
tajiri1999 1:e49ca9ffcccb 71 bool check_voltage(); //電圧をチェックする
tajiri1999 1:e49ca9ffcccb 72 void uart_send(unsigned char data);
tajiri1999 1:e49ca9ffcccb 73 unsigned char uart_rec(void);
tajiri1999 1:e49ca9ffcccb 74 unsigned char skps(unsigned char data);
tajiri1999 1:e49ca9ffcccb 75 unsigned char skps_vibrate(unsigned char motor, unsigned char value);
tajiri1999 1:e49ca9ffcccb 76 float convert_degree(float x,float y);
tajiri1999 1:e49ca9ffcccb 77 float convert_length(float x,float y);
tajiri1999 1:e49ca9ffcccb 78 /*****************************************************************/
tajiri1999 1:e49ca9ffcccb 79 /**********************main function******************************/
tajiri1999 1:e49ca9ffcccb 80 /*****************************************************************/
tajiri1999 0:6c2e6a485519 81
tajiri1999 0:6c2e6a485519 82 int main()
tajiri1999 0:6c2e6a485519 83 {
tajiri1999 0:6c2e6a485519 84
tajiri1999 1:e49ca9ffcccb 85 //**************************************************************//
tajiri1999 1:e49ca9ffcccb 86 ////////////////////////initialize setting////////////////////////
tajiri1999 1:e49ca9ffcccb 87 //**************************************************************//
tajiri1999 1:e49ca9ffcccb 88 wait_ms(50);
tajiri1999 1:e49ca9ffcccb 89 kick = 0; //キックを解除する
tajiri1999 0:6c2e6a485519 90
tajiri1999 1:e49ca9ffcccb 91 /*motor pwm frequency set*/
tajiri1999 1:e49ca9ffcccb 92 motor.setPwmPeriod(0.00052);
tajiri1999 0:6c2e6a485519 93
tajiri1999 1:e49ca9ffcccb 94 while (1) {
tajiri1999 1:e49ca9ffcccb 95 //***************************************************************//
tajiri1999 1:e49ca9ffcccb 96 ////////////////Play mode////////////
tajiri1999 1:e49ca9ffcccb 97 //***************************************************************//
tajiri1999 1:e49ca9ffcccb 98 timer_volt.reset();
tajiri1999 1:e49ca9ffcccb 99 timer_volt.start();
tajiri1999 1:e49ca9ffcccb 100 while (sw_start == 0 && check_voltage() == 1) {
tajiri1999 1:e49ca9ffcccb 101 float deg,power,yaw_angle,lx,ly,rx;
tajiri1999 1:e49ca9ffcccb 102
tajiri1999 1:e49ca9ffcccb 103 //convert int to float
tajiri1999 1:e49ca9ffcccb 104 lx = (float)skps(p_joy_lx);
tajiri1999 1:e49ca9ffcccb 105 ly = (float)skps(p_joy_ly);
tajiri1999 1:e49ca9ffcccb 106 rx = (float)skps(p_joy_rx);
tajiri1999 1:e49ca9ffcccb 107 deg = convert_degree(lx,ly);
tajiri1999 1:e49ca9ffcccb 108
tajiri1999 1:e49ca9ffcccb 109 yaw_angle = (rx - 128)/128;
tajiri1999 1:e49ca9ffcccb 110
tajiri1999 2:e1fedc61e5e3 111 power = convert_length(lx,ly)*0.7;
tajiri1999 1:e49ca9ffcccb 112 if(power <= 10) { //遊びをカット
tajiri1999 1:e49ca9ffcccb 113 power = 0;
tajiri1999 0:6c2e6a485519 114 }
tajiri1999 1:e49ca9ffcccb 115
tajiri1999 1:e49ca9ffcccb 116 if(skps(p_l1) == 0 || skps(p_square) == 0){
tajiri1999 1:e49ca9ffcccb 117 motor.omniWheels(deg,power,yaw_angle*30);
tajiri1999 0:6c2e6a485519 118 }
tajiri1999 1:e49ca9ffcccb 119 else{
tajiri1999 1:e49ca9ffcccb 120 motor.omniWheels(deg,power*0.5,yaw_angle*30);
tajiri1999 1:e49ca9ffcccb 121 }
tajiri1999 1:e49ca9ffcccb 122 /*kick*/
tajiri1999 1:e49ca9ffcccb 123 if(skps(p_circle) == 0 || skps(p_r1)== 0){
tajiri1999 1:e49ca9ffcccb 124 kick = 1;
tajiri1999 1:e49ca9ffcccb 125 wait_ms(100);
tajiri1999 1:e49ca9ffcccb 126 kick = 0;
tajiri1999 0:6c2e6a485519 127 }
tajiri1999 0:6c2e6a485519 128 }
tajiri1999 1:e49ca9ffcccb 129 motor.omniWheels(0, 0, 0);
tajiri1999 1:e49ca9ffcccb 130 /**********************end main function**************************/
tajiri1999 0:6c2e6a485519 131 }
tajiri1999 0:6c2e6a485519 132 }
tajiri1999 0:6c2e6a485519 133
tajiri1999 0:6c2e6a485519 134
tajiri1999 1:e49ca9ffcccb 135 //////////////////////////////////////
tajiri1999 1:e49ca9ffcccb 136 /*Battery check voltage*/
tajiri1999 1:e49ca9ffcccb 137 //////////////////////////////////////
tajiri1999 1:e49ca9ffcccb 138 bool check_voltage()
tajiri1999 0:6c2e6a485519 139 {
tajiri1999 1:e49ca9ffcccb 140 static float sum[5] = { 8, 8, 8, 8, 8 };
tajiri1999 1:e49ca9ffcccb 141 static float Ave = 8;
tajiri1999 1:e49ca9ffcccb 142 static bool S = 1;
tajiri1999 1:e49ca9ffcccb 143 if (timer_volt.read() > 0.2) {
tajiri1999 1:e49ca9ffcccb 144 for (int i = 4; i > 0; i--) {
tajiri1999 1:e49ca9ffcccb 145 sum[i] = sum[i - 1];
tajiri1999 1:e49ca9ffcccb 146 }
tajiri1999 1:e49ca9ffcccb 147 sum[0] = voltage.read() * 9.9;
tajiri1999 1:e49ca9ffcccb 148 Ave = (sum[0] + sum[1] + sum[2] + sum[3] + sum[4]) / 5;
tajiri1999 1:e49ca9ffcccb 149 if (Ave < 7.0) { //7.0V以下で自動遮断
tajiri1999 1:e49ca9ffcccb 150 S = 0;
tajiri1999 1:e49ca9ffcccb 151 } else {
tajiri1999 1:e49ca9ffcccb 152 S = 1;
tajiri1999 0:6c2e6a485519 153
tajiri1999 0:6c2e6a485519 154 }
tajiri1999 1:e49ca9ffcccb 155 timer_volt.reset();
tajiri1999 0:6c2e6a485519 156 }
tajiri1999 1:e49ca9ffcccb 157 return S;
tajiri1999 0:6c2e6a485519 158 }
tajiri1999 0:6c2e6a485519 159
tajiri1999 1:e49ca9ffcccb 160 void uart_send(unsigned char data)
tajiri1999 0:6c2e6a485519 161 {
tajiri1999 1:e49ca9ffcccb 162 ps2.putc(data);
tajiri1999 1:e49ca9ffcccb 163 }
tajiri1999 0:6c2e6a485519 164
tajiri1999 1:e49ca9ffcccb 165 unsigned char uart_rec(void)
tajiri1999 1:e49ca9ffcccb 166 {
tajiri1999 1:e49ca9ffcccb 167 unsigned char temp;
tajiri1999 1:e49ca9ffcccb 168 while(ps2.readable() == 0);
tajiri1999 1:e49ca9ffcccb 169 temp = ps2.getc();
tajiri1999 1:e49ca9ffcccb 170 return temp;
tajiri1999 0:6c2e6a485519 171 }
tajiri1999 0:6c2e6a485519 172
tajiri1999 1:e49ca9ffcccb 173 unsigned char skps(unsigned char data)
tajiri1999 0:6c2e6a485519 174 {
tajiri1999 1:e49ca9ffcccb 175 uart_send(data);
tajiri1999 1:e49ca9ffcccb 176 return uart_rec();
tajiri1999 1:e49ca9ffcccb 177 }
tajiri1999 0:6c2e6a485519 178
tajiri1999 1:e49ca9ffcccb 179 unsigned char skps_vibrate(unsigned char motor, unsigned char value)
tajiri1999 1:e49ca9ffcccb 180 {
tajiri1999 1:e49ca9ffcccb 181 uart_send(motor);
tajiri1999 1:e49ca9ffcccb 182 uart_send(value);
tajiri1999 0:6c2e6a485519 183 }
tajiri1999 0:6c2e6a485519 184
tajiri1999 1:e49ca9ffcccb 185 /**************************************************************/
tajiri1999 1:e49ca9ffcccb 186 /*関数名：convert_degree(float x,float y)*/
tajiri1999 1:e49ca9ffcccb 187 /*引数：ジョイスティックのX,Y Axis*/
tajiri1999 1:e49ca9ffcccb 188 /*戻り値：float型　極座標の角度(前方が0度,時計回りに360度)*/
tajiri1999 1:e49ca9ffcccb 189 /*概要:ロボットの進行方向を決めるときに使う*/
tajiri1999 1:e49ca9ffcccb 190 /*************************************************************/
tajiri1999 1:e49ca9ffcccb 191 float convert_degree(float x,float y){
tajiri1999 1:e49ca9ffcccb 192 float degree;
tajiri1999 1:e49ca9ffcccb 193 degree = (180/PI)*atan2(y - 128,x - 128) + 360 + 90;//オイラー角度に変換
tajiri1999 1:e49ca9ffcccb 194
tajiri1999 1:e49ca9ffcccb 195 //角度を0~360度に変換
tajiri1999 1:e49ca9ffcccb 196 if(degree >= 360.0){
tajiri1999 1:e49ca9ffcccb 197 degree = degree - 360;
tajiri1999 1:e49ca9ffcccb 198 }
tajiri1999 1:e49ca9ffcccb 199 return degree;
tajiri1999 0:6c2e6a485519 200 }
tajiri1999 0:6c2e6a485519 201
tajiri1999 1:e49ca9ffcccb 202 /***************************************************************/
tajiri1999 1:e49ca9ffcccb 203 /*関数名:convert_length(float x,flaot y)*/
tajiri1999 1:e49ca9ffcccb 204 /*引数:ジョイスティックのX,Y*/
tajiri1999 1:e49ca9ffcccb 205 /*戻り値：float型 極座標の原点から(x,y)点までの長さを0~100で返す*/
tajiri1999 1:e49ca9ffcccb 206 /*概要:モーターのパワーを決めるときに,極座標の長さを使う*/
tajiri1999 1:e49ca9ffcccb 207 /***************************************************************/
tajiri1999 1:e49ca9ffcccb 208 float convert_length(float x,float y){
tajiri1999 1:e49ca9ffcccb 209 float xy_length;
tajiri1999 1:e49ca9ffcccb 210 xy_length = pow(x - 128,2) + pow(y - 128,2);
tajiri1999 1:e49ca9ffcccb 211 xy_length = (100 * xy_length)/(128*128);
tajiri1999 1:e49ca9ffcccb 212 if(xy_length >= 100.0){
tajiri1999 1:e49ca9ffcccb 213 xy_length = 100.0;
tajiri1999 1:e49ca9ffcccb 214 }
tajiri1999 1:e49ca9ffcccb 215 return xy_length;
tajiri1999 0:6c2e6a485519 216 }