ハヤト タジリ
/
MAID_ROBOT_makerfair
スネーク
main.cpp@1:e49ca9ffcccb, 2020-03-23 (annotated)
- Committer:
- tajiri1999
- Date:
- Mon Mar 23 11:30:19 2020 +0000
- Revision:
- 1:e49ca9ffcccb
- Parent:
- 0:6c2e6a485519
- Child:
- 2:e1fedc61e5e3
g
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:e49ca9ffcccb | 111 | power = convert_length(lx,ly); |
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 | } |