2015_robocon_bteam / Mbed 2 deprecated 2015robot_main

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   PID QEI mbed

Fork of 2015robot_main by Naoto Deguchi

main_ps3.cpp@6:a350810d2071, 2015-09-07 (annotated)

Committer:
unicore32
Date:
Mon Sep 07 05:10:54 2015 +0000
Revision:
6:a350810d2071
Parent:
4:51d87d2b698c
Child:
7:de01f9ca78f8
??RS485??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:bd4719e15f7e 1 /**
DeguNaoto 0:bd4719e15f7e 2 * This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
DeguNaoto 0:bd4719e15f7e 3 */
DeguNaoto 2:cf8ca6742db9 4
DeguNaoto 0:bd4719e15f7e 5 #include "machine_ps3.h"
DeguNaoto 0:bd4719e15f7e 6
DeguNaoto 0:bd4719e15f7e 7 Serial pc(USBTX, USBRX);
DeguNaoto 0:bd4719e15f7e 8
DeguNaoto 0:bd4719e15f7e 9 //LocalFileSystem local("local");
DeguNaoto 0:bd4719e15f7e 10
DeguNaoto 2:cf8ca6742db9 11 int main()
DeguNaoto 2:cf8ca6742db9 12 {
DeguNaoto 2:cf8ca6742db9 13
DeguNaoto 2:cf8ca6742db9 14 //FILE *fp_d = fopen("/local/out_d.dat", "w");
DeguNaoto 4:51d87d2b698c 15 wait(0.1);
DeguNaoto 0:bd4719e15f7e 16 initializeSBDBT();
DeguNaoto 4:51d87d2b698c 17 wait(0.1);
DeguNaoto 4:51d87d2b698c 18 while(analog_ly==0)
DeguNaoto 4:51d87d2b698c 19 wait(0.5); //room in his heart.
DeguNaoto 0:bd4719e15f7e 20 initializeMotors();
DeguNaoto 0:bd4719e15f7e 21 initializeControllers();
DeguNaoto 4:51d87d2b698c 22 //RS485.putc(10);
unicore32 6:a350810d2071 23 sendData(5, 10);
DeguNaoto 0:bd4719e15f7e 24 while(1) {
DeguNaoto 4:51d87d2b698c 25 /*if(circle) Emergency_toggle();
DeguNaoto 4:51d87d2b698c 26 else if(triangle){
DeguNaoto 4:51d87d2b698c 27 //targ_swing_velocity=1000;
DeguNaoto 4:51d87d2b698c 28 Motor_swing_pwm = 0.4;
DeguNaoto 4:51d87d2b698c 29 }
DeguNaoto 4:51d87d2b698c 30 else if(square){
DeguNaoto 4:51d87d2b698c 31 //targ_swing_velocity=500;
DeguNaoto 4:51d87d2b698c 32 Motor_swing_pwm = 0.5;
DeguNaoto 4:51d87d2b698c 33 }
DeguNaoto 4:51d87d2b698c 34 else if(down) Motor_swing_pwm = 0.6;
DeguNaoto 4:51d87d2b698c 35 else if(left) Motor_swing_pwm = 0.7;
DeguNaoto 4:51d87d2b698c 36 else if(up) Motor_swing_pwm = 0.8;
DeguNaoto 4:51d87d2b698c 37 else if(right) Motor_swing_pwm = 0.9;
DeguNaoto 4:51d87d2b698c 38 else if(r1) Motor_swing_pwm = 1.0;
DeguNaoto 4:51d87d2b698c 39 else if(cross){
DeguNaoto 4:51d87d2b698c 40 Motor_swing_pwm = 0.0;
DeguNaoto 4:51d87d2b698c 41 //targ_swing_velocity=0;
DeguNaoto 4:51d87d2b698c 42 //swing_controller.reset();
DeguNaoto 4:51d87d2b698c 43 }*/
DeguNaoto 4:51d87d2b698c 44 //mesure_swing_velocity();
DeguNaoto 4:51d87d2b698c 45 //Swing_speed_following();
DeguNaoto 4:51d87d2b698c 46 short exc=0;
DeguNaoto 4:51d87d2b698c 47 if(analog_ry>90) Move_r(-0.38),exc=1;
DeguNaoto 4:51d87d2b698c 48 else if(analog_ry<40) Move_r(0.33),exc=1;
DeguNaoto 4:51d87d2b698c 49 if(analog_ly>90) Move_l(-0.38),exc=1;
DeguNaoto 4:51d87d2b698c 50 else if(analog_ly<40) Move_l(0.38),exc=1;
DeguNaoto 4:51d87d2b698c 51 //////////////////////////////////////////////////////////////
DeguNaoto 4:51d87d2b698c 52 /*if(up) {
DeguNaoto 0:bd4719e15f7e 53 //move_r_controller.setTunings(2.0,10.0,0.0);
DeguNaoto 0:bd4719e15f7e 54 //move_l_controller.setTunings(2.0,10.0,0.0);
DeguNaoto 0:bd4719e15f7e 55 move_r_controller.reset();
DeguNaoto 0:bd4719e15f7e 56 move_l_controller.reset();
DeguNaoto 0:bd4719e15f7e 57 targ_move_r_velocity=50.0;
DeguNaoto 0:bd4719e15f7e 58 targ_move_l_velocity=50.0;
DeguNaoto 2:cf8ca6742db9 59 } else if(down) {
DeguNaoto 0:bd4719e15f7e 60 //move_r_controller.setTunings(2.0,10.0,0.0);
DeguNaoto 0:bd4719e15f7e 61 //move_l_controller.setTunings(2.0,10.0,0.0);
DeguNaoto 0:bd4719e15f7e 62 move_r_controller.reset();
DeguNaoto 0:bd4719e15f7e 63 move_l_controller.reset();
DeguNaoto 0:bd4719e15f7e 64 targ_move_r_velocity=-50.0;
DeguNaoto 0:bd4719e15f7e 65 targ_move_l_velocity=-50.0;
DeguNaoto 4:51d87d2b698c 66 }*/
DeguNaoto 4:51d87d2b698c 67 //////////////////////////////////////////////////////////////
DeguNaoto 4:51d87d2b698c 68 if(up) {
DeguNaoto 4:51d87d2b698c 69 if(edge_up){
DeguNaoto 4:51d87d2b698c 70 edge_up=0;
DeguNaoto 4:51d87d2b698c 71 if(cylinder_step==3) cylinder_step=0;
DeguNaoto 4:51d87d2b698c 72 cylinder_step++;
unicore32 6:a350810d2071 73 sendData(1, cylinder_step);
unicore32 6:a350810d2071 74 //RS485.putc(cylinder_step);
DeguNaoto 4:51d87d2b698c 75 wait(0.5);
unicore32 6:a350810d2071 76 sendData(1, 4);
unicore32 6:a350810d2071 77 //RS485.putc(4);
DeguNaoto 4:51d87d2b698c 78 }
DeguNaoto 4:51d87d2b698c 79 }
DeguNaoto 4:51d87d2b698c 80 else if(down) step=7;
unicore32 6:a350810d2071 81 else if(right) sendData(5, 11);//RS485.putc(11);
unicore32 6:a350810d2071 82 else if(left) sendData(1, 10);//RS485.putc(10);
DeguNaoto 4:51d87d2b698c 83 /*else if(right) {
DeguNaoto 4:51d87d2b698c 84 if(edge_right){
DeguNaoto 4:51d87d2b698c 85 edge_right=0;
DeguNaoto 4:51d87d2b698c 86 targ_move_r_velocity+=2.0;
DeguNaoto 4:51d87d2b698c 87 }
DeguNaoto 2:cf8ca6742db9 88 } else if(left) {
DeguNaoto 4:51d87d2b698c 89 if(edge_left){
DeguNaoto 4:51d87d2b698c 90 edge_left=0;
DeguNaoto 4:51d87d2b698c 91 targ_move_l_velocity+=2.0;
DeguNaoto 4:51d87d2b698c 92 }
DeguNaoto 4:51d87d2b698c 93 } */
DeguNaoto 4:51d87d2b698c 94 /*else if(r1) {
DeguNaoto 0:bd4719e15f7e 95 move_r_controller.reset();
DeguNaoto 0:bd4719e15f7e 96 move_l_controller.reset();
DeguNaoto 0:bd4719e15f7e 97 targ_move_r_velocity=-50.0;
DeguNaoto 0:bd4719e15f7e 98 targ_move_l_velocity=50.0;
DeguNaoto 0:bd4719e15f7e 99 } else if(l1) {
DeguNaoto 0:bd4719e15f7e 100 move_r_controller.reset();
DeguNaoto 0:bd4719e15f7e 101 move_l_controller.reset();
DeguNaoto 0:bd4719e15f7e 102 targ_move_r_velocity=50.0;
DeguNaoto 2:cf8ca6742db9 103 targ_move_l_velocity=-50.0;
DeguNaoto 4:51d87d2b698c 104 } */
DeguNaoto 4:51d87d2b698c 105 /////////////////////////////////////////////
DeguNaoto 4:51d87d2b698c 106 else if(r1) {
DeguNaoto 4:51d87d2b698c 107 if(edge_r1){
DeguNaoto 4:51d87d2b698c 108 edge_r1=0;
unicore32 6:a350810d2071 109 //RS485.putc(5);
unicore32 6:a350810d2071 110 sendData(1, 5);
DeguNaoto 4:51d87d2b698c 111 wait(0.5);
unicore32 6:a350810d2071 112 //RS485.putc(7);
unicore32 6:a350810d2071 113 sendData(1, 7);
DeguNaoto 4:51d87d2b698c 114 }
DeguNaoto 4:51d87d2b698c 115 } else if(l1) {
DeguNaoto 4:51d87d2b698c 116 if(edge_l1){
DeguNaoto 4:51d87d2b698c 117 edge_l1=0;
unicore32 6:a350810d2071 118 //RS485.putc(6);
unicore32 6:a350810d2071 119 sendData(1, 6);
DeguNaoto 4:51d87d2b698c 120 wait(0.5);
unicore32 6:a350810d2071 121 //RS485.putc(7);
unicore32 6:a350810d2071 122 sendData(1, 7);
DeguNaoto 4:51d87d2b698c 123 }
DeguNaoto 4:51d87d2b698c 124 }
DeguNaoto 4:51d87d2b698c 125 /////////////////////////////////////////////
DeguNaoto 4:51d87d2b698c 126 /*else if(r2) {
DeguNaoto 0:bd4719e15f7e 127 move_r_controller.reset();
DeguNaoto 0:bd4719e15f7e 128 move_l_controller.reset();
DeguNaoto 0:bd4719e15f7e 129 targ_move_r_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 130 targ_move_l_velocity=50.0;
DeguNaoto 0:bd4719e15f7e 131 } else if(l2) {
DeguNaoto 0:bd4719e15f7e 132 move_r_controller.reset();
DeguNaoto 0:bd4719e15f7e 133 move_l_controller.reset();
DeguNaoto 0:bd4719e15f7e 134 targ_move_r_velocity=50.0;
DeguNaoto 0:bd4719e15f7e 135 targ_move_l_velocity=0.0;
DeguNaoto 4:51d87d2b698c 136 }*/ else if(cross) {
DeguNaoto 2:cf8ca6742db9 137 move_r_controller.reset();
DeguNaoto 2:cf8ca6742db9 138 move_l_controller.reset();
DeguNaoto 0:bd4719e15f7e 139 targ_move_r_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 140 targ_move_l_velocity=0.0;
DeguNaoto 2:cf8ca6742db9 141 Move_r_sense.reset();
DeguNaoto 2:cf8ca6742db9 142 Move_l_sense.reset();
DeguNaoto 4:51d87d2b698c 143 step=0;
DeguNaoto 2:cf8ca6742db9 144 //fclose(fp_r);
DeguNaoto 2:cf8ca6742db9 145 } else if(square) {
DeguNaoto 0:bd4719e15f7e 146 move_r_controller.reset();
DeguNaoto 0:bd4719e15f7e 147 move_l_controller.reset();
DeguNaoto 0:bd4719e15f7e 148 targ_move_r_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 149 targ_move_l_velocity=0.0;
DeguNaoto 2:cf8ca6742db9 150 } else if(circle) {
DeguNaoto 0:bd4719e15f7e 151 Emergency_toggle();
DeguNaoto 4:51d87d2b698c 152 }
DeguNaoto 4:51d87d2b698c 153 else if(triangle){
DeguNaoto 4:51d87d2b698c 154 move_step();
DeguNaoto 0:bd4719e15f7e 155 }
DeguNaoto 4:51d87d2b698c 156 if(step==0){
DeguNaoto 4:51d87d2b698c 157 targ_move_r_velocity=0.0;
DeguNaoto 4:51d87d2b698c 158 targ_move_l_velocity=0.0;
DeguNaoto 2:cf8ca6742db9 159 move_r_controller.reset();
DeguNaoto 2:cf8ca6742db9 160 move_l_controller.reset();
DeguNaoto 4:51d87d2b698c 161 }
DeguNaoto 4:51d87d2b698c 162 else if(step==1){
DeguNaoto 4:51d87d2b698c 163 targ_move_r_velocity=60.0;
DeguNaoto 4:51d87d2b698c 164 targ_move_l_velocity=60.0;
DeguNaoto 4:51d87d2b698c 165 if(((long long)Move_r_sense.getPulses()>3000)||(((long long)Move_l_sense.getPulses())>3000)){
DeguNaoto 4:51d87d2b698c 166 targ_move_r_velocity=0.0;
DeguNaoto 4:51d87d2b698c 167 targ_move_l_velocity=0.0;
DeguNaoto 4:51d87d2b698c 168 wait(0.3);
DeguNaoto 4:51d87d2b698c 169 Move_r_sense.reset();
DeguNaoto 4:51d87d2b698c 170 Move_l_sense.reset();
DeguNaoto 4:51d87d2b698c 171 //step++;
DeguNaoto 4:51d87d2b698c 172 pc.printf("%d",step);
DeguNaoto 4:51d87d2b698c 173 }
DeguNaoto 4:51d87d2b698c 174 }
DeguNaoto 4:51d87d2b698c 175 else if(step==2){
DeguNaoto 4:51d87d2b698c 176 targ_move_r_velocity=-60.0;
DeguNaoto 4:51d87d2b698c 177 targ_move_l_velocity=0.0;
DeguNaoto 4:51d87d2b698c 178 if((long long)Move_r_sense.getPulses()<-500){
DeguNaoto 4:51d87d2b698c 179 targ_move_r_velocity=0.0;
DeguNaoto 4:51d87d2b698c 180 targ_move_l_velocity=0.0;
DeguNaoto 4:51d87d2b698c 181 wait(0.3);
DeguNaoto 4:51d87d2b698c 182 Move_r_sense.reset();
DeguNaoto 4:51d87d2b698c 183 Move_l_sense.reset();
DeguNaoto 4:51d87d2b698c 184 //step++;
DeguNaoto 4:51d87d2b698c 185 pc.printf("%d",step);
DeguNaoto 4:51d87d2b698c 186 }
DeguNaoto 4:51d87d2b698c 187 }
DeguNaoto 4:51d87d2b698c 188 else if(step==3){
DeguNaoto 4:51d87d2b698c 189 targ_move_r_velocity=60.0;
DeguNaoto 4:51d87d2b698c 190 targ_move_l_velocity=60.0;
DeguNaoto 4:51d87d2b698c 191 if(((long long)Move_r_sense.getPulses()>10000)||(((long long)Move_l_sense.getPulses())>10000)){
DeguNaoto 4:51d87d2b698c 192 targ_move_r_velocity=0.0;
DeguNaoto 4:51d87d2b698c 193 targ_move_l_velocity=0.0;
DeguNaoto 4:51d87d2b698c 194 wait(0.3);
DeguNaoto 4:51d87d2b698c 195 Move_r_sense.reset();
DeguNaoto 4:51d87d2b698c 196 Move_l_sense.reset();
DeguNaoto 4:51d87d2b698c 197 //step++;
DeguNaoto 4:51d87d2b698c 198 pc.printf("%d",step);
DeguNaoto 4:51d87d2b698c 199 }
DeguNaoto 4:51d87d2b698c 200 }
DeguNaoto 4:51d87d2b698c 201 else if(step==4){
DeguNaoto 4:51d87d2b698c 202 targ_move_r_velocity=60.0;
DeguNaoto 2:cf8ca6742db9 203 targ_move_l_velocity=0.0;
DeguNaoto 4:51d87d2b698c 204 if((long long)Move_r_sense.getPulses()>500){
DeguNaoto 4:51d87d2b698c 205 targ_move_r_velocity=0.0;
DeguNaoto 4:51d87d2b698c 206 targ_move_l_velocity=0.0;
DeguNaoto 4:51d87d2b698c 207 wait(0.3);
DeguNaoto 4:51d87d2b698c 208 Move_r_sense.reset();
DeguNaoto 4:51d87d2b698c 209 Move_l_sense.reset();
DeguNaoto 4:51d87d2b698c 210 //step++;
DeguNaoto 4:51d87d2b698c 211 pc.printf("%d",step);
DeguNaoto 4:51d87d2b698c 212 }
DeguNaoto 4:51d87d2b698c 213 }
DeguNaoto 4:51d87d2b698c 214 else if(step==5){
DeguNaoto 4:51d87d2b698c 215 targ_move_r_velocity=-60.0;
DeguNaoto 4:51d87d2b698c 216 targ_move_l_velocity=-60.0;
DeguNaoto 4:51d87d2b698c 217 if(((long long)Move_r_sense.getPulses()<-5000)||(((long long)Move_l_sense.getPulses())<-5000)){
DeguNaoto 4:51d87d2b698c 218 targ_move_r_velocity=0.0;
DeguNaoto 4:51d87d2b698c 219 targ_move_l_velocity=0.0;
DeguNaoto 4:51d87d2b698c 220 wait(0.3);
DeguNaoto 4:51d87d2b698c 221 Move_r_sense.reset();
DeguNaoto 4:51d87d2b698c 222 Move_l_sense.reset();
DeguNaoto 4:51d87d2b698c 223 //step++;
DeguNaoto 4:51d87d2b698c 224 pc.printf("%d",step);
DeguNaoto 4:51d87d2b698c 225 }
DeguNaoto 4:51d87d2b698c 226 }
DeguNaoto 4:51d87d2b698c 227 else if(step==6){
DeguNaoto 4:51d87d2b698c 228 targ_move_r_velocity=0.0;
DeguNaoto 4:51d87d2b698c 229 targ_move_l_velocity=60.0;
DeguNaoto 4:51d87d2b698c 230 if((long long)Move_l_sense.getPulses()>500){
DeguNaoto 4:51d87d2b698c 231 targ_move_r_velocity=0.0;
DeguNaoto 4:51d87d2b698c 232 targ_move_l_velocity=0.0;
DeguNaoto 4:51d87d2b698c 233 wait(0.3);
DeguNaoto 4:51d87d2b698c 234 Move_r_sense.reset();
DeguNaoto 4:51d87d2b698c 235 Move_l_sense.reset();
DeguNaoto 4:51d87d2b698c 236 //step++;
DeguNaoto 4:51d87d2b698c 237 pc.printf("%d",step);
DeguNaoto 4:51d87d2b698c 238 }
DeguNaoto 4:51d87d2b698c 239 }
DeguNaoto 4:51d87d2b698c 240 else if(step==7){
DeguNaoto 4:51d87d2b698c 241 targ_move_r_velocity=-60.0;
DeguNaoto 4:51d87d2b698c 242 targ_move_l_velocity=-60.0;
DeguNaoto 4:51d87d2b698c 243 if(((long long)Move_r_sense.getPulses()<-10000)||(((long long)Move_l_sense.getPulses())<-10000)){
DeguNaoto 4:51d87d2b698c 244 //move_r_controller.reset();
DeguNaoto 4:51d87d2b698c 245 //move_l_controller.reset();
DeguNaoto 4:51d87d2b698c 246 targ_move_r_velocity=0.0;
DeguNaoto 4:51d87d2b698c 247 targ_move_l_velocity=0.0;
DeguNaoto 4:51d87d2b698c 248 wait(0.3);
DeguNaoto 4:51d87d2b698c 249 Move_r_sense.reset();
DeguNaoto 4:51d87d2b698c 250 Move_l_sense.reset();
DeguNaoto 4:51d87d2b698c 251 //step=0;
DeguNaoto 4:51d87d2b698c 252 pc.printf("%d",step);
DeguNaoto 4:51d87d2b698c 253 }
DeguNaoto 4:51d87d2b698c 254 }
DeguNaoto 4:51d87d2b698c 255 if(!exc){
DeguNaoto 4:51d87d2b698c 256 mesure_move_l_velocity();
DeguNaoto 4:51d87d2b698c 257 mesure_move_r_velocity();
DeguNaoto 4:51d87d2b698c 258 Move_r_speed_following();
DeguNaoto 4:51d87d2b698c 259 Move_l_speed_following();
DeguNaoto 4:51d87d2b698c 260 }
DeguNaoto 4:51d87d2b698c 261 pc.printf("ry is:%d ,ly is:%d ,rx is %d ,lx is:%d\r\n", analog_ry, analog_ly, analog_rx, analog_lx);
DeguNaoto 0:bd4719e15f7e 262 //pc.printf("targ is:%ld ,velocity is:%ld ,cont is %f\r\n", targ_swing_velocity, swing_velocity, cont_swing);
DeguNaoto 4:51d87d2b698c 263 //pc.printf("velocity r is:%f\r\n",swing_velocity);
DeguNaoto 4:51d87d2b698c 264 //pc.printf("compute swing is:%f\r\n",cont_swing);
DeguNaoto 4:51d87d2b698c 265 //pc.printf("compute r is:%f,compute l is:%f\r\n",cont_move_r,cont_move_l);
DeguNaoto 4:51d87d2b698c 266 //pc.printf("sens r is:%ld ,sens l is:%ld\r\n",Move_r_sense.getPulses(),Move_l_sense.getPulses());
DeguNaoto 4:51d87d2b698c 267 //pc.printf("swing sens is:%ld\r\n",Swing_speed_sense.getPulses());
DeguNaoto 2:cf8ca6742db9 268 //fprintf(fp_r, "%ld\r\n",move_r_velocity);
DeguNaoto 0:bd4719e15f7e 269 wait(RATE);
DeguNaoto 0:bd4719e15f7e 270 }
DeguNaoto 0:bd4719e15f7e 271 }