Ryu Anzawa / Mbed OS 2019NHK_A_manual_red

to takasou

Dependencies:   SerialMultiByte QEI chatteringremoval omni_wheel prototype01 towelest PID ikarashiMDC_2byte_ver air2 OmniPosition PS3

main.cpp@2:d01bc3dcb247, 2019-09-27 (annotated)

Committer:
piroro4560
Date:
Fri Sep 27 10:50:13 2019 +0000
Revision:
2:d01bc3dcb247
Parent:
1:e7413a7b013b 
aa;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piroro4560 2:d01bc3dcb247 1 //#include "mbed.h" //Red court
piroro4560 2:d01bc3dcb247 2 //#include "ikarashiMDC.h"
piroro4560 2:d01bc3dcb247 3 //#include "omni_wheel.h"
piroro4560 2:d01bc3dcb247 4 //#include "proto01.h"
piroro4560 2:d01bc3dcb247 5 //#include "OmniPosition.h"
piroro4560 2:d01bc3dcb247 6 //#include "SerialMultiByte.h"
piroro4560 2:d01bc3dcb247 7 //#include "PID.h"
piroro4560 2:d01bc3dcb247 8 //#include "PS3.h"
piroro4560 2:d01bc3dcb247 9 //#include "air.h"
piroro4560 2:d01bc3dcb247 10 //#include "towelest.h"
piroro4560 2:d01bc3dcb247 11 //#include "chatteringremoval.h"
piroro4560 2:d01bc3dcb247 12 //#include "pinconfig_main.h"
piroro4560 2:d01bc3dcb247 13 //
piroro4560 2:d01bc3dcb247 14 //PID angle(10.0,0.2,0.000095,0.01);
piroro4560 2:d01bc3dcb247 15 //OmniWheel omni(4);
piroro4560 2:d01bc3dcb247 16 //OmniPosition posi(main_0TX,main_0RX);
piroro4560 2:d01bc3dcb247 17 //Serial serial(mdTX, mdRX, 115200);
piroro4560 2:d01bc3dcb247 18 //RawSerial pc(USBTX, USBRX, 115200);
piroro4560 2:d01bc3dcb247 19 //SerialMultiByte sensor(main_1TX,main_1RX);
piroro4560 2:d01bc3dcb247 20 //towelest towel(&serial,2,schmitt_trigger_0,schmitt_trigger_1,schmitt_trigger_2);
piroro4560 2:d01bc3dcb247 21 //air air;
piroro4560 2:d01bc3dcb247 22 //PS3 ps3(FEPTX,FEPRX);
piroro4560 2:d01bc3dcb247 23 //chatteringremoval button1(1.0);
piroro4560 2:d01bc3dcb247 24 //DigitalOut sw1(hijoteisi);
piroro4560 2:d01bc3dcb247 25 //DigitalOut LED(LED1);
piroro4560 2:d01bc3dcb247 26 //Timer timer;
piroro4560 2:d01bc3dcb247 27 //
piroro4560 2:d01bc3dcb247 28 //int num = 0, X, Y, b[12], stick[4], trigger[2], loli[3], b+[12];
piroro4560 2:d01bc3dcb247 29 //int X0,Y0,X1,Y1;
piroro4560 2:d01bc3dcb247 30 //double valueX, valueY, value[4],mecha_power[4],spin_power=0, IRdistance[2],TFdistance[2],x2, y2;
piroro4560 2:d01bc3dcb247 31 //
piroro4560 2:d01bc3dcb247 32 //union UnionBytes{
piroro4560 2:d01bc3dcb247 33 // uint8_t bytes[28];
piroro4560 2:d01bc3dcb247 34 // float IRdist[7]; //0~1
piroro4560 2:d01bc3dcb247 35 // int32_t TFdist[7]; //2~3
piroro4560 2:d01bc3dcb247 36 // int32_t pulses[7]; //4~6
piroro4560 2:d01bc3dcb247 37 //};
piroro4560 2:d01bc3dcb247 38 //
piroro4560 2:d01bc3dcb247 39 //ikarashiMDC motor[]=
piroro4560 2:d01bc3dcb247 40 //{
piroro4560 2:d01bc3dcb247 41 // ikarashiMDC(0,0,SM,&serial),
piroro4560 2:d01bc3dcb247 42 // ikarashiMDC(0,1,SM,&serial),
piroro4560 2:d01bc3dcb247 43 // ikarashiMDC(0,2,SM,&serial),
piroro4560 2:d01bc3dcb247 44 // ikarashiMDC(0,3,SM,&serial),
piroro4560 2:d01bc3dcb247 45 // ikarashiMDC(1,0,SM,&serial),
piroro4560 2:d01bc3dcb247 46 // ikarashiMDC(1,1,SM,&serial),
piroro4560 2:d01bc3dcb247 47 // ikarashiMDC(1,2,SM,&serial),
piroro4560 2:d01bc3dcb247 48 // ikarashiMDC(1,3,SM,&serial)
piroro4560 2:d01bc3dcb247 49 //};
piroro4560 2:d01bc3dcb247 50 //
piroro4560 2:d01bc3dcb247 51 //Proto1 proto(500, 700, 0.4, 0.14);
piroro4560 2:d01bc3dcb247 52 //
piroro4560 2:d01bc3dcb247 53 //
piroro4560 2:d01bc3dcb247 54 //void getSensorData() //Receive sensor value
piroro4560 2:d01bc3dcb247 55 //{
piroro4560 2:d01bc3dcb247 56 // UnionBytes bytedata;
piroro4560 2:d01bc3dcb247 57 // sensor.getData(bytedata.bytes);
piroro4560 2:d01bc3dcb247 58 // for(int i=0; i<2; i++){
piroro4560 2:d01bc3dcb247 59 // IRdistance[i] = bytedata.IRdist[i];
piroro4560 2:d01bc3dcb247 60 // }
piroro4560 2:d01bc3dcb247 61 // for(int i=0; i<2; i++){
piroro4560 2:d01bc3dcb247 62 // TFdistance[i] = bytedata.TFdist[i+2];
piroro4560 2:d01bc3dcb247 63 // }
piroro4560 2:d01bc3dcb247 64 // for(int i=0; i<3; i++){
piroro4560 2:d01bc3dcb247 65 // loli[i] = bytedata.pulses[i+4];
piroro4560 2:d01bc3dcb247 66 // }
piroro4560 2:d01bc3dcb247 67 //}
piroro4560 2:d01bc3dcb247 68 //
piroro4560 2:d01bc3dcb247 69 //void clossmove0() //Adjustment,Airfront
piroro4560 2:d01bc3dcb247 70 //{
piroro4560 2:d01bc3dcb247 71 // if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
piroro4560 2:d01bc3dcb247 72 // x2=0;
piroro4560 2:d01bc3dcb247 73 // y2=0.3;
piroro4560 2:d01bc3dcb247 74 // }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
piroro4560 2:d01bc3dcb247 75 // x2=0;
piroro4560 2:d01bc3dcb247 76 // y2=-0.3;
piroro4560 2:d01bc3dcb247 77 // }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
piroro4560 2:d01bc3dcb247 78 // x2=-0.3;
piroro4560 2:d01bc3dcb247 79 // y2=0;
piroro4560 2:d01bc3dcb247 80 // }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
piroro4560 2:d01bc3dcb247 81 // x2=0.3;
piroro4560 2:d01bc3dcb247 82 // y2=0;
piroro4560 2:d01bc3dcb247 83 // }else{
piroro4560 2:d01bc3dcb247 84 // x2=0;
piroro4560 2:d01bc3dcb247 85 // y2=0;
piroro4560 2:d01bc3dcb247 86 // }
piroro4560 2:d01bc3dcb247 87 //}
piroro4560 2:d01bc3dcb247 88 //
piroro4560 2:d01bc3dcb247 89 //void clossmove1() //Adjustment,Recoveryfront
piroro4560 2:d01bc3dcb247 90 //{
piroro4560 2:d01bc3dcb247 91 // if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
piroro4560 2:d01bc3dcb247 92 // x2=0.3;
piroro4560 2:d01bc3dcb247 93 // y2=0;
piroro4560 2:d01bc3dcb247 94 // }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
piroro4560 2:d01bc3dcb247 95 // x2=-0.3;
piroro4560 2:d01bc3dcb247 96 // y2=0;
piroro4560 2:d01bc3dcb247 97 // }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
piroro4560 2:d01bc3dcb247 98 // x2=0;
piroro4560 2:d01bc3dcb247 99 // y2=0.3;
piroro4560 2:d01bc3dcb247 100 // }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
piroro4560 2:d01bc3dcb247 101 // x2=0;
piroro4560 2:d01bc3dcb247 102 // y2=-0.3;
piroro4560 2:d01bc3dcb247 103 // }else{
piroro4560 2:d01bc3dcb247 104 // x2=0;
piroro4560 2:d01bc3dcb247 105 // y2=0;
piroro4560 2:d01bc3dcb247 106 // }
piroro4560 2:d01bc3dcb247 107 //}
piroro4560 2:d01bc3dcb247 108 //
piroro4560 2:d01bc3dcb247 109 //void clossmove2() //Adjustment,Dropfront
piroro4560 2:d01bc3dcb247 110 //{
piroro4560 2:d01bc3dcb247 111 // if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
piroro4560 2:d01bc3dcb247 112 // x2=0;
piroro4560 2:d01bc3dcb247 113 // y2=-0.3;
piroro4560 2:d01bc3dcb247 114 // }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
piroro4560 2:d01bc3dcb247 115 // x2=0;
piroro4560 2:d01bc3dcb247 116 // y2=0.3;
piroro4560 2:d01bc3dcb247 117 // }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
piroro4560 2:d01bc3dcb247 118 // x2=0.3;
piroro4560 2:d01bc3dcb247 119 // y2=0;
piroro4560 2:d01bc3dcb247 120 // }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
piroro4560 2:d01bc3dcb247 121 // x2=-0.3;
piroro4560 2:d01bc3dcb247 122 // y2=0;
piroro4560 2:d01bc3dcb247 123 // }else{
piroro4560 2:d01bc3dcb247 124 // x2=0;
piroro4560 2:d01bc3dcb247 125 // y2=0;
piroro4560 2:d01bc3dcb247 126 // }
piroro4560 2:d01bc3dcb247 127 //}
piroro4560 2:d01bc3dcb247 128 //
piroro4560 2:d01bc3dcb247 129 //void clossmove3() //Adjustment,Towelfront
piroro4560 2:d01bc3dcb247 130 //{
piroro4560 2:d01bc3dcb247 131 // if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
piroro4560 2:d01bc3dcb247 132 // x2=-0.3;
piroro4560 2:d01bc3dcb247 133 // y2=0;
piroro4560 2:d01bc3dcb247 134 // }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
piroro4560 2:d01bc3dcb247 135 // x2=0.3;
piroro4560 2:d01bc3dcb247 136 // y2=0;
piroro4560 2:d01bc3dcb247 137 // }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
piroro4560 2:d01bc3dcb247 138 // x2=0;
piroro4560 2:d01bc3dcb247 139 // y2=-0.3;
piroro4560 2:d01bc3dcb247 140 // }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
piroro4560 2:d01bc3dcb247 141 // x2=0;
piroro4560 2:d01bc3dcb247 142 // y2=0.3;
piroro4560 2:d01bc3dcb247 143 // }else{
piroro4560 2:d01bc3dcb247 144 // x2=0;
piroro4560 2:d01bc3dcb247 145 // y2=0;
piroro4560 2:d01bc3dcb247 146 // }
piroro4560 2:d01bc3dcb247 147 //}
piroro4560 2:d01bc3dcb247 148 //
piroro4560 2:d01bc3dcb247 149 //void Recovery() //Recovery mechanism
piroro4560 2:d01bc3dcb247 150 //{
piroro4560 2:d01bc3dcb247 151 // /* 洗濯物を挟む機構 */
piroro4560 2:d01bc3dcb247 152 // if((X0 > 170)&&(Y0 > 50 && Y0 < 200)) {
piroro4560 2:d01bc3dcb247 153 // mecha_power[1] = 0.6;
piroro4560 2:d01bc3dcb247 154 // }else if((X0 < 100)&&(Y0 > 50 && Y0 < 200)) {
piroro4560 2:d01bc3dcb247 155 // mecha_power[1] = -0.6;
piroro4560 2:d01bc3dcb247 156 // }else{
piroro4560 2:d01bc3dcb247 157 // mecha_power[1] = 0;
piroro4560 2:d01bc3dcb247 158 // }
piroro4560 2:d01bc3dcb247 159 // if((X1 > 170)&&(Y1 > 50 && Y1 < 200)) {
piroro4560 2:d01bc3dcb247 160 // mecha_power[2] = 0.6;
piroro4560 2:d01bc3dcb247 161 // }else if((X1 < 100)&&(Y1 > 50 && Y1 < 200)) {
piroro4560 2:d01bc3dcb247 162 // mecha_power[2] = -0.6;
piroro4560 2:d01bc3dcb247 163 // }else{
piroro4560 2:d01bc3dcb247 164 // mecha_power[2] = 0;
piroro4560 2:d01bc3dcb247 165 // }
piroro4560 2:d01bc3dcb247 166 // /* 洗濯物を挟む機構の首 */
piroro4560 2:d01bc3dcb247 167 // if (b[10] == 1 && b[11] == 0) {
piroro4560 2:d01bc3dcb247 168 // mecha_power[0] = 0.3;
piroro4560 2:d01bc3dcb247 169 // } else if (b[10] == 0 && b[11] == 1) {
piroro4560 2:d01bc3dcb247 170 // mecha_power[0] = -0.3;
piroro4560 2:d01bc3dcb247 171 // } else {
piroro4560 2:d01bc3dcb247 172 // mecha_power[0] =0.0;
piroro4560 2:d01bc3dcb247 173 // }
piroro4560 2:d01bc3dcb247 174 // mecha_power[3] = 0;
piroro4560 2:d01bc3dcb247 175 //
piroro4560 2:d01bc3dcb247 176 // motor[4].setSpeed(mecha_power[0]);
piroro4560 2:d01bc3dcb247 177 // motor[5].setSpeed(mecha_power[1]);
piroro4560 2:d01bc3dcb247 178 // motor[6].setSpeed(mecha_power[2]);
piroro4560 2:d01bc3dcb247 179 // motor[7].setSpeed(mecha_power[3]);
piroro4560 2:d01bc3dcb247 180 //}
piroro4560 2:d01bc3dcb247 181 //
piroro4560 2:d01bc3dcb247 182 //void stop()
piroro4560 2:d01bc3dcb247 183 //{
piroro4560 2:d01bc3dcb247 184 // for(int i=4;i<8;i++) motor[i].setSpeed(0);
piroro4560 2:d01bc3dcb247 185 //}
piroro4560 2:d01bc3dcb247 186 //
piroro4560 2:d01bc3dcb247 187 //void Drop()
piroro4560 2:d01bc3dcb247 188 //{
piroro4560 2:d01bc3dcb247 189 // if(b[4] == 1 && b[5] == 0){
piroro4560 2:d01bc3dcb247 190 // mecha_power[3] = 0.5;
piroro4560 2:d01bc3dcb247 191 // }else if(b[4] == 0 && b[5] == 1) {
piroro4560 2:d01bc3dcb247 192 // mecha_power[3] = -0.5;
piroro4560 2:d01bc3dcb247 193 // }else{
piroro4560 2:d01bc3dcb247 194 // mecha_power[3] = 0;
piroro4560 2:d01bc3dcb247 195 // }
piroro4560 2:d01bc3dcb247 196 // motor[7].setSpeed(mecha_power[3]);
piroro4560 2:d01bc3dcb247 197 //}
piroro4560 2:d01bc3dcb247 198 //
piroro4560 2:d01bc3dcb247 199 //int main()
piroro4560 2:d01bc3dcb247 200 //{
piroro4560 2:d01bc3dcb247 201 // sw1 = 1;
piroro4560 2:d01bc3dcb247 202 // double nowX,nowY,t;
piroro4560 2:d01bc3dcb247 203 //
piroro4560 2:d01bc3dcb247 204 // for(int i=0;i<4;i++)motor[i].braking = false;
piroro4560 2:d01bc3dcb247 205 // omni.wheel[0].setRadian(PI * 1 / 4);
piroro4560 2:d01bc3dcb247 206 // omni.wheel[1].setRadian(PI * 3 / 4);
piroro4560 2:d01bc3dcb247 207 // omni.wheel[2].setRadian(PI * 5 / 4);
piroro4560 2:d01bc3dcb247 208 // omni.wheel[3].setRadian(PI * 7 / 4);
piroro4560 2:d01bc3dcb247 209 // angle.setInputLimits(-6.28,6.28);
piroro4560 2:d01bc3dcb247 210 // angle.setOutputLimits(-0.4,0.4);
piroro4560 2:d01bc3dcb247 211 // angle.setBias(0);
piroro4560 2:d01bc3dcb247 212 // angle.setMode(1);
piroro4560 2:d01bc3dcb247 213 // angle.setSetPoint(0);
piroro4560 2:d01bc3dcb247 214 //
piroro4560 2:d01bc3dcb247 215 // button1.countreset();
piroro4560 2:d01bc3dcb247 216 //
piroro4560 2:d01bc3dcb247 217 // sensor.startReceive(28);
piroro4560 2:d01bc3dcb247 218 //
piroro4560 2:d01bc3dcb247 219 // while(1){
piroro4560 2:d01bc3dcb247 220 // LED = !LED;
piroro4560 2:d01bc3dcb247 221 //
piroro4560 2:d01bc3dcb247 222 // towel.setPulse(loli[2]);
piroro4560 2:d01bc3dcb247 223 //
piroro4560 2:d01bc3dcb247 224 //// PS3 value
piroro4560 2:d01bc3dcb247 225 // for(int i = 0; i < 12; i++) {
piroro4560 2:d01bc3dcb247 226 // b[i] = ps3.getButton(i);
piroro4560 2:d01bc3dcb247 227 // pc.printf("%2d", b[i] );
piroro4560 2:d01bc3dcb247 228 // }
piroro4560 2:d01bc3dcb247 229 // for(int i = 0; i < 4; i++) {
piroro4560 2:d01bc3dcb247 230 // stick[i] = ps3.getStick(i);
piroro4560 2:d01bc3dcb247 231 // pc.printf("%4d", stick[i] );
piroro4560 2:d01bc3dcb247 232 // }
piroro4560 2:d01bc3dcb247 233 // X0 = stick[0];
piroro4560 2:d01bc3dcb247 234 // Y0 = stick[1];
piroro4560 2:d01bc3dcb247 235 // X1 = stick[2];
piroro4560 2:d01bc3dcb247 236 // Y1 = stick[3];
piroro4560 2:d01bc3dcb247 237 //
piroro4560 2:d01bc3dcb247 238 // for(int i = 0; i < 2; i++) {
piroro4560 2:d01bc3dcb247 239 // trigger[i] = ps3.getTrigger(i);
piroro4560 2:d01bc3dcb247 240 //// pc.printf("%4d",trigger[i]);
piroro4560 2:d01bc3dcb247 241 // }
piroro4560 2:d01bc3dcb247 242 //
piroro4560 2:d01bc3dcb247 243 // //Sensor value
piroro4560 2:d01bc3dcb247 244 // getSensorData();
piroro4560 2:d01bc3dcb247 245 //
piroro4560 2:d01bc3dcb247 246 // //Count
piroro4560 2:d01bc3dcb247 247 // button1.assignvalue(b[9]);
piroro4560 2:d01bc3dcb247 248 // num = button1.getCount();
piroro4560 2:d01bc3dcb247 249 //
piroro4560 2:d01bc3dcb247 250 // /*【Action】*/
piroro4560 2:d01bc3dcb247 251 // switch(num){
piroro4560 2:d01bc3dcb247 252 // case 0: angle.setSetPoint(0);
piroro4560 2:d01bc3dcb247 253 // clossmove0();
piroro4560 2:d01bc3dcb247 254 // Recovery();
piroro4560 2:d01bc3dcb247 255 // towel.open();
piroro4560 2:d01bc3dcb247 256 // air.solenoid1_open();
piroro4560 2:d01bc3dcb247 257 // nowX = posi.getX();
piroro4560 2:d01bc3dcb247 258 // nowY = posi.getY();
piroro4560 2:d01bc3dcb247 259 // break;
piroro4560 2:d01bc3dcb247 260 // case 1: proto.targetXY(-3000, 0, nowX ,nowY);
piroro4560 2:d01bc3dcb247 261 // towel.allstop();
piroro4560 2:d01bc3dcb247 262 // stop();
piroro4560 2:d01bc3dcb247 263 // break;
piroro4560 2:d01bc3dcb247 264 // case 2: angle.setSetPoint(1.57);
piroro4560 2:d01bc3dcb247 265 // Drop();
piroro4560 2:d01bc3dcb247 266 // clossmove3();
piroro4560 2:d01bc3dcb247 267 // towel.allstop();
piroro4560 2:d01bc3dcb247 268 // stop();
piroro4560 2:d01bc3dcb247 269 // nowX = posi.getX();
piroro4560 2:d01bc3dcb247 270 // nowY = posi.getY();
piroro4560 2:d01bc3dcb247 271 // break;
piroro4560 2:d01bc3dcb247 272 // case 3: proto.targetXY(-2000, 3000, nowX ,nowY);
piroro4560 2:d01bc3dcb247 273 // towel.allstop();
piroro4560 2:d01bc3dcb247 274 // stop();
piroro4560 2:d01bc3dcb247 275 // if((posi.getX()>-2200 && posi.getX()<-1800) && (posi.getY()>2800 && posi.getY()<3200)){
piroro4560 2:d01bc3dcb247 276 // num = 4;
piroro4560 2:d01bc3dcb247 277 // angle.setSetPoint(0);
piroro4560 2:d01bc3dcb247 278 // }
piroro4560 2:d01bc3dcb247 279 // break;
piroro4560 2:d01bc3dcb247 280 // case 4: clossmove0();
piroro4560 2:d01bc3dcb247 281 //// Recovery();
piroro4560 2:d01bc3dcb247 282 //// nowX = posi.getX();
piroro4560 2:d01bc3dcb247 283 //// nowY = posi.getY();
piroro4560 2:d01bc3dcb247 284 //// break;
piroro4560 2:d01bc3dcb247 285 //// case 5: proto.targetXY(-3000, 0, nowX ,nowY);
piroro4560 2:d01bc3dcb247 286 //// break;
piroro4560 2:d01bc3dcb247 287 //// case 6: angle.setSetPoint(1.57);
piroro4560 2:d01bc3dcb247 288 //// Drop();
piroro4560 2:d01bc3dcb247 289 //// clossmove3();
piroro4560 2:d01bc3dcb247 290 //// nowX = posi.getX();
piroro4560 2:d01bc3dcb247 291 //// nowY = posi.getY();
piroro4560 2:d01bc3dcb247 292 //// break;
piroro4560 2:d01bc3dcb247 293 //// case 7: proto.targetXY(-3000,2600,-3000,0);
piroro4560 2:d01bc3dcb247 294 //// if(posi.getX()>-3200 && posi.getX()<-2800 && posi.getY()>2400 && posi.getY()<2800){
piroro4560 2:d01bc3dcb247 295 //// num = 8;
piroro4560 2:d01bc3dcb247 296 //// }
piroro4560 2:d01bc3dcb247 297 //// break;
piroro4560 2:d01bc3dcb247 298 //// case 8: angle.setSetPoint(1.57);
piroro4560 2:d01bc3dcb247 299 //// clossmove3();
piroro4560 2:d01bc3dcb247 300 //// nowX = posi.getX();
piroro4560 2:d01bc3dcb247 301 //// nowY = posi.getY();
piroro4560 2:d01bc3dcb247 302 ////
piroro4560 2:d01bc3dcb247 303 //// towel.drop();
piroro4560 2:d01bc3dcb247 304 //// t = timer.read();
piroro4560 2:d01bc3dcb247 305 //// if(b[4] == 1 && b[5] == 1){
piroro4560 2:d01bc3dcb247 306 //// timer.start();
piroro4560 2:d01bc3dcb247 307 //// }
piroro4560 2:d01bc3dcb247 308 //// if(t > 5){
piroro4560 2:d01bc3dcb247 309 //// towel.close();
piroro4560 2:d01bc3dcb247 310 //// }else if(t > 20){
piroro4560 2:d01bc3dcb247 311 //// angle.setSetPoint(0);
piroro4560 2:d01bc3dcb247 312 //// air.solenoid2_open();
piroro4560 2:d01bc3dcb247 313 //// }
piroro4560 2:d01bc3dcb247 314 //// break;
piroro4560 2:d01bc3dcb247 315 //// case 9: angle.setSetPoint(0);
piroro4560 2:d01bc3dcb247 316 //// proto.targetXY(0, 0,nowX ,nowY);
piroro4560 2:d01bc3dcb247 317 //// break;
piroro4560 2:d01bc3dcb247 318 // default: motor[0].setSpeed(0);
piroro4560 2:d01bc3dcb247 319 // motor[1].setSpeed(0);
piroro4560 2:d01bc3dcb247 320 // motor[2].setSpeed(0);
piroro4560 2:d01bc3dcb247 321 // motor[3].setSpeed(0);
piroro4560 2:d01bc3dcb247 322 //
piroro4560 2:d01bc3dcb247 323 // air.solenoid1_close();
piroro4560 2:d01bc3dcb247 324 // air.solenoid2_close();
piroro4560 2:d01bc3dcb247 325 //
piroro4560 2:d01bc3dcb247 326 // }
piroro4560 2:d01bc3dcb247 327 //
piroro4560 2:d01bc3dcb247 328 // angle.setProcessValue(posi.getTheta());
piroro4560 2:d01bc3dcb247 329 // spin_power = -angle.compute();
piroro4560 2:d01bc3dcb247 330 //
piroro4560 2:d01bc3dcb247 331 // if(num == 0 || num == 2 || num == 4 || num == 6 || num == 8){
piroro4560 2:d01bc3dcb247 332 // omni.computeXY(x2,y2,spin_power);
piroro4560 2:d01bc3dcb247 333 // }else{
piroro4560 2:d01bc3dcb247 334 // proto.Input_nowXY(posi.getX(),posi.getY());
piroro4560 2:d01bc3dcb247 335 // proto.calculate();
piroro4560 2:d01bc3dcb247 336 // omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
piroro4560 2:d01bc3dcb247 337 // }
piroro4560 2:d01bc3dcb247 338 // /*
piroro4560 2:d01bc3dcb247 339 // omni.conputeCircular(proto.getValue(), proto.gettheta(), spin_power);
piroro4560 2:d01bc3dcb247 340 // */
piroro4560 2:d01bc3dcb247 341 //
piroro4560 2:d01bc3dcb247 342 // for(int i = 0; i < 4; i++){
piroro4560 2:d01bc3dcb247 343 // value[i] = omni.wheel[i];
piroro4560 2:d01bc3dcb247 344 // motor[i].setSpeed(value[i]);
piroro4560 2:d01bc3dcb247 345 //// pc.printf("%f ", value[i]);
piroro4560 2:d01bc3dcb247 346 // }
piroro4560 2:d01bc3dcb247 347 //
piroro4560 2:d01bc3dcb247 348 //// wait(0.001);
piroro4560 2:d01bc3dcb247 349 // pc.printf(" loli:%d case:%d",loli[2],num);
piroro4560 2:d01bc3dcb247 350 //
piroro4560 2:d01bc3dcb247 351 // //Wheel value
piroro4560 2:d01bc3dcb247 352 //// pc.printf(" vector:%f pid:%.2f",proto.vector,spin_power);
piroro4560 2:d01bc3dcb247 353 // //Odmetry
piroro4560 2:d01bc3dcb247 354 //// pc.printf("x:%d y:%d θ:%f now:%f",posi.getX(),posi.getY(),posi.getTheta(),proto.nowDis);
piroro4560 2:d01bc3dcb247 355 //
piroro4560 2:d01bc3dcb247 356 // pc.printf("\r\n");
piroro4560 2:d01bc3dcb247 357 // for(int i = 0; i < 12; i++) {
piroro4560 2:d01bc3dcb247 358 // b+[i] = ps3.getButton(i);
piroro4560 2:d01bc3dcb247 359 // }
piroro4560 2:d01bc3dcb247 360 // }
piroro4560 2:d01bc3dcb247 361 //}
piroro4560 2:d01bc3dcb247 362 #include "mbed.h" //たかそうのロボット
piroro4560 2:d01bc3dcb247 363 #include "ikarashiMDC.h"
ec30109b 0:d3db2b0afc76 364 #include "omni_wheel.h"
ec30109b 0:d3db2b0afc76 365 #include "proto01.h"
ec30109b 0:d3db2b0afc76 366 #include "OmniPosition.h"
ec30109b 0:d3db2b0afc76 367 #include "PID.h"
ec30109b 0:d3db2b0afc76 368 #include "pinconfig_main.h"
ec30109b 0:d3db2b0afc76 369
piroro4560 2:d01bc3dcb247 370 PID angle(20.0,5.0,0.0000001,0.01);
ec30109b 0:d3db2b0afc76 371 OmniWheel omni(4);
piroro4560 2:d01bc3dcb247 372 OmniPosition posi(main_0TX, main_0RX);
ec30109b 0:d3db2b0afc76 373 Serial serial(mdTX, mdRX, 115200);
piroro4560 2:d01bc3dcb247 374 DigitalOut serialcontrol(D10);
ec30109b 0:d3db2b0afc76 375 RawSerial pc(USBTX, USBRX, 115200);
piroro4560 2:d01bc3dcb247 376 DigitalOut sw1(PB_1);
piroro4560 2:d01bc3dcb247 377 DigitalOut sw2();
ec30109b 0:d3db2b0afc76 378
ec30109b 0:d3db2b0afc76 379 ikarashiMDC motor[]=
ec30109b 0:d3db2b0afc76 380 {
ec30109b 0:d3db2b0afc76 381 ikarashiMDC(0,0,SM,&serial),
ec30109b 0:d3db2b0afc76 382 ikarashiMDC(0,1,SM,&serial),
ec30109b 0:d3db2b0afc76 383 ikarashiMDC(0,2,SM,&serial),
piroro4560 2:d01bc3dcb247 384 ikarashiMDC(0,3,SM,&serial)
ec30109b 0:d3db2b0afc76 385 };
ec30109b 0:d3db2b0afc76 386
piroro4560 2:d01bc3dcb247 387 Proto1 proto(500, 500, 0.4, 0.14);
ec30109b 0:d3db2b0afc76 388
ec30109b 0:d3db2b0afc76 389 int main()
ec30109b 0:d3db2b0afc76 390 {
ec30109b 0:d3db2b0afc76 391 sw1 = 1;
piroro4560 2:d01bc3dcb247 392 int cnt=0;
piroro4560 2:d01bc3dcb247 393 motor[0].braking = true;
piroro4560 2:d01bc3dcb247 394 double valueX, valueY, value[4];
ec30109b 0:d3db2b0afc76 395 omni.wheel[0].setRadian(PI * 1 / 4);
ec30109b 0:d3db2b0afc76 396 omni.wheel[1].setRadian(PI * 3 / 4);
ec30109b 0:d3db2b0afc76 397 omni.wheel[2].setRadian(PI * 5 / 4);
ec30109b 0:d3db2b0afc76 398 omni.wheel[3].setRadian(PI * 7 / 4);
piroro4560 2:d01bc3dcb247 399 angle.setInputLimits(-3.14,3.14);
ec30109b 0:d3db2b0afc76 400 angle.setOutputLimits(-0.4,0.4);
ec30109b 0:d3db2b0afc76 401 angle.setBias(0);
ec30109b 0:d3db2b0afc76 402 angle.setMode(1);
ec30109b 0:d3db2b0afc76 403 angle.setSetPoint(0);
ec30109b 0:d3db2b0afc76 404 while(1){
ec30109b 0:d3db2b0afc76 405 angle.setProcessValue(posi.getTheta());
piroro4560 2:d01bc3dcb247 406 double spin_power = -angle.compute();
piroro4560 2:d01bc3dcb247 407 /*ここから*/
piroro4560 2:d01bc3dcb247 408 if (cnt==700) proto.targetXY(2000, 3000, posi.getX(), -posi.getY());
piroro4560 2:d01bc3dcb247 409 if (cnt==1400) proto.targetXY(2000, 6000, posi.getX(), -posi.getY());
piroro4560 2:d01bc3dcb247 410 if (cnt==2100) proto.targetXY(0, 6000, posi.getX(), -posi.getY());
piroro4560 2:d01bc3dcb247 411 if (cnt==2800) proto.targetXY(0, 3000, posi.getX(), -posi.getY());
piroro4560 2:d01bc3dcb247 412 if (cnt==3500) proto.targetXY(0, 0, posi.getX(), -posi.getY());
piroro4560 2:d01bc3dcb247 413 // if (cnt==3000) proto.targetXY(0, 0, posi.getX(), posi.getY());
piroro4560 2:d01bc3dcb247 414 proto.Input_nowXY(posi.getX(), -posi.getY());
piroro4560 2:d01bc3dcb247 415 proto.calculate();
piroro4560 2:d01bc3dcb247 416 omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
piroro4560 2:d01bc3dcb247 417 /*ここまで*/
ec30109b 0:d3db2b0afc76 418
ec30109b 0:d3db2b0afc76 419 for(int i = 0; i < 4; i++){
ec30109b 0:d3db2b0afc76 420 value[i] = omni.wheel[i];
piroro4560 2:d01bc3dcb247 421 // pc.printf("%.2f || ",value[i]);
ec30109b 0:d3db2b0afc76 422 }
piroro4560 2:d01bc3dcb247 423 pc.printf("x:%d || y:%d || θ:%f || vector:%f || now:%f || c:%f"
piroro4560 2:d01bc3dcb247 424 , posi.getX(), -posi.getY(), posi.getTheta(), proto.vector, proto.nowDis, proto.counter);
ec30109b 0:d3db2b0afc76 425
piroro4560 2:d01bc3dcb247 426 for(int i = 0; i < 4; i++) {
piroro4560 2:d01bc3dcb247 427 if (fabs(value[i]) < 0.05) value[i] = 0;
piroro4560 2:d01bc3dcb247 428 motor[i].setSpeed(value[i]);
piroro4560 1:e7413a7b013b 429 }
piroro4560 2:d01bc3dcb247 430 cnt++;
piroro4560 2:d01bc3dcb247 431 pc.printf("||pid:%.2f\r\n",spin_power);
ec30109b 0:d3db2b0afc76 432 }
piroro4560 2:d01bc3dcb247 433 }