robocon_2016 / Mbed 2 deprecated 2016_mother_3

mother

Dependencies:   PS3 i2cmaster mbed

main.cpp@1:755d8977d4a3, 2016-09-30 (annotated)

Committer:
sgrsn
Date:
Fri Sep 30 12:29:16 2016 +0000
Revision:
1:755d8977d4a3
Parent:
0:a6b6c585a378 
mother_9/30

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:a6b6c585a378 1 #include "mbed.h"
sgrsn 0:a6b6c585a378 2 #include "PS3.h"
sgrsn 0:a6b6c585a378 3 #include "i2cmaster.h"
sgrsn 0:a6b6c585a378 4 #include "define.h"
sgrsn 0:a6b6c585a378 5
sgrsn 0:a6b6c585a378 6 #define Enter 13
sgrsn 0:a6b6c585a378 7 #define BackSpace 8
sgrsn 0:a6b6c585a378 8 #define Space 32
sgrsn 0:a6b6c585a378 9
sgrsn 0:a6b6c585a378 10 i2c myi2c(p28, p27);
sgrsn 0:a6b6c585a378 11
sgrsn 0:a6b6c585a378 12 int i2ccheck[4] = {};
sgrsn 0:a6b6c585a378 13 int timer = 0;
sgrsn 0:a6b6c585a378 14
sgrsn 0:a6b6c585a378 15 void check()
sgrsn 0:a6b6c585a378 16 {
sgrsn 0:a6b6c585a378 17 timer++;
sgrsn 0:a6b6c585a378 18 /*Slave puted data first by mother cause error*/
sgrsn 0:a6b6c585a378 19 myi2c.WriteSomeData(0xFE, check_reg, 0, 1); //0xFE is imaginary　address
sgrsn 0:a6b6c585a378 20 /*then, I puted imaginary address yhe!*/
sgrsn 0:a6b6c585a378 21 myi2c.WriteSomeData(MD1_addr, check_reg, 0, 1);
sgrsn 0:a6b6c585a378 22 myi2c.WriteSomeData(MD2_addr, check_reg, 0, 1);
sgrsn 0:a6b6c585a378 23 myi2c.WriteSomeData(MD3_addr, check_reg, 0, 1);
sgrsn 0:a6b6c585a378 24 myi2c.WriteSomeData(MD4_addr, check_reg, 0, 1);
sgrsn 0:a6b6c585a378 25 myi2c.WriteSomeData(MD5_addr, check_reg, 0, 1);
sgrsn 0:a6b6c585a378 26 myi2c.WriteSomeData(MD6_addr, check_reg, 0, 1);
sgrsn 0:a6b6c585a378 27 myi2c.WriteSomeData(MD7_addr, check_reg, 0, 1);
sgrsn 0:a6b6c585a378 28 myi2c.WriteSomeData(MD8_addr, check_reg, 0, 1);
sgrsn 0:a6b6c585a378 29 myi2c.WriteSomeData(MD9_addr, check_reg, 0, 1);
sgrsn 0:a6b6c585a378 30 myi2c.WriteSomeData(MD10_addr, check_reg, 0, 1);
sgrsn 0:a6b6c585a378 31 myi2c.WriteSomeData(SE1_addr, check_reg, 0, 1);
sgrsn 0:a6b6c585a378 32 myi2c.WriteSomeData(SE2_addr, check_reg, 0, 1);
sgrsn 0:a6b6c585a378 33 myi2c.WriteSomeData(CY1_addr, check_reg, 0, 1);
sgrsn 1:755d8977d4a3 34 myi2c.WriteSomeData(SERVO1_addr, check_reg, 0, 1);
sgrsn 1:755d8977d4a3 35 myi2c.WriteSomeData(0xFE, check_reg, 0, 1);
sgrsn 0:a6b6c585a378 36 }
sgrsn 0:a6b6c585a378 37
sgrsn 0:a6b6c585a378 38 union UNION
sgrsn 0:a6b6c585a378 39 {
sgrsn 0:a6b6c585a378 40 int integer;
sgrsn 0:a6b6c585a378 41 char character[4];
sgrsn 0:a6b6c585a378 42 };
sgrsn 0:a6b6c585a378 43 UNION PM_target[4];
sgrsn 0:a6b6c585a378 44 UNION PM_ofset[4];
sgrsn 0:a6b6c585a378 45 UNION RE_target[4];
sgrsn 0:a6b6c585a378 46 UNION gain;
sgrsn 0:a6b6c585a378 47
sgrsn 0:a6b6c585a378 48 Serial pc(USBTX, USBRX);
sgrsn 0:a6b6c585a378 49 int pcdata = 0;
sgrsn 0:a6b6c585a378 50 int prev_pcdata = 0;
sgrsn 0:a6b6c585a378 51 void getpc();
sgrsn 0:a6b6c585a378 52
sgrsn 0:a6b6c585a378 53 DigitalOut sbdbt_battery(p21);
sgrsn 0:a6b6c585a378 54 DigitalOut xbee_battery(p5);
sgrsn 0:a6b6c585a378 55
sgrsn 0:a6b6c585a378 56 int main()
sgrsn 0:a6b6c585a378 57 {
sgrsn 1:755d8977d4a3 58 BusOut leds(LED1, LED2, LED3, LED4);
sgrsn 0:a6b6c585a378 59 NVIC_SetPriority(TIMER0_IRQn, 20);
sgrsn 0:a6b6c585a378 60 NVIC_SetPriority(TIMER1_IRQn, 20);
sgrsn 0:a6b6c585a378 61 NVIC_SetPriority(TIMER2_IRQn, 20);
sgrsn 0:a6b6c585a378 62 NVIC_SetPriority(TIMER3_IRQn, 20);
sgrsn 0:a6b6c585a378 63 NVIC_SetPriority(UART0_IRQn, 10);
sgrsn 0:a6b6c585a378 64 NVIC_SetPriority(UART1_IRQn, 10);
sgrsn 0:a6b6c585a378 65 NVIC_SetPriority(UART2_IRQn, 10);
sgrsn 0:a6b6c585a378 66 NVIC_SetPriority(UART3_IRQn, 10);
sgrsn 0:a6b6c585a378 67 PS3 sbdbt(p13, p14);
sgrsn 0:a6b6c585a378 68 PS3 xbee(p9, p10);
sgrsn 0:a6b6c585a378 69 sbdbt_battery = 1;
sgrsn 0:a6b6c585a378 70 sbdbt.baud(9600);
sgrsn 0:a6b6c585a378 71 xbee_battery = 1;
sgrsn 0:a6b6c585a378 72 xbee.baud(9600);
sgrsn 0:a6b6c585a378 73 //xbee.addattach(sbdbt_to_xbee);
sgrsn 0:a6b6c585a378 74 //sbdbt.addattach(xbee_to_sbdbt);
sgrsn 0:a6b6c585a378 75 pc.attach(getpc);
sgrsn 0:a6b6c585a378 76 myi2c.frequency(1000000);
sgrsn 0:a6b6c585a378 77 Ticker t;
sgrsn 0:a6b6c585a378 78 t.attach(check, 0.1);
sgrsn 0:a6b6c585a378 79 int prev_PM_target[4] = {};
sgrsn 0:a6b6c585a378 80 char sensor1 = 0;
sgrsn 0:a6b6c585a378 81 char sensors[8] = {};
sgrsn 0:a6b6c585a378 82 char sensorsflag[8] = {};
sgrsn 0:a6b6c585a378 83 char sensor2 = 0;
sgrsn 0:a6b6c585a378 84 int gainup = 0;
sgrsn 0:a6b6c585a378 85 int Dpad_deg = 0;
sgrsn 0:a6b6c585a378 86 bool Dpadctrl_flag = 1;
sgrsn 0:a6b6c585a378 87 //0, ue, sita
sgrsn 0:a6b6c585a378 88 int Dpad_table[3][3] = {{ 0, 0, 180}, //0
sgrsn 0:a6b6c585a378 89 { 90, 45, 135}, //migi
sgrsn 0:a6b6c585a378 90 {-90, -45, -135}};//hidari
sgrsn 0:a6b6c585a378 91 int wheel[4] = {};
sgrsn 0:a6b6c585a378 92 int torikomi_taosu_motor = 0;
sgrsn 0:a6b6c585a378 93 int modechange = 0;
sgrsn 0:a6b6c585a378 94 int modechangeflag = 1;
sgrsn 0:a6b6c585a378 95 bool counterwheel = 0;
sgrsn 0:a6b6c585a378 96 bool l1 = 0;
sgrsn 0:a6b6c585a378 97 bool r1 = 0;
sgrsn 0:a6b6c585a378 98 bool l2 = 0;
sgrsn 0:a6b6c585a378 99 bool r2 = 0;
sgrsn 0:a6b6c585a378 100 bool Maru = 0;
sgrsn 0:a6b6c585a378 101 bool Batu = 0;
sgrsn 0:a6b6c585a378 102 bool Sankaku = 0;
sgrsn 0:a6b6c585a378 103 int sankakuflag = 0;
sgrsn 0:a6b6c585a378 104 bool Sikaku = 0;
sgrsn 0:a6b6c585a378 105 bool Ue = 0;
sgrsn 0:a6b6c585a378 106 bool Sita = 0;
sgrsn 0:a6b6c585a378 107 bool Migi = 0;
sgrsn 0:a6b6c585a378 108 bool Hidari = 0;
sgrsn 0:a6b6c585a378 109 int RightY = 0;
sgrsn 0:a6b6c585a378 110 int LeftY = 0;
sgrsn 0:a6b6c585a378 111 int LeftX = 0;
sgrsn 0:a6b6c585a378 112 wait_ms(500);
sgrsn 0:a6b6c585a378 113
sgrsn 0:a6b6c585a378 114 for(int address = 0; address < 4; address++)
sgrsn 0:a6b6c585a378 115 {
sgrsn 0:a6b6c585a378 116 while(PM_ofset[address].integer < 1750 || PM_ofset[address].integer > 1850)
sgrsn 0:a6b6c585a378 117 {
sgrsn 0:a6b6c585a378 118 for(int i = 0; i < 4; i++)
sgrsn 0:a6b6c585a378 119 {
sgrsn 0:a6b6c585a378 120 myi2c.getSlaveRegistarData(MD1_addr + (0x02*address), PM_ofset_reg + i, &PM_ofset[address].character[i], 1);
sgrsn 0:a6b6c585a378 121 wait_ms(20);
sgrsn 0:a6b6c585a378 122 }
sgrsn 0:a6b6c585a378 123 }
sgrsn 0:a6b6c585a378 124 PM_target[address].integer = PM_ofset[address].integer;
sgrsn 0:a6b6c585a378 125 }
sgrsn 0:a6b6c585a378 126 while(1)
sgrsn 0:a6b6c585a378 127 {
sgrsn 1:755d8977d4a3 128 leds = modechange;
sgrsn 1:755d8977d4a3 129 if( modechangeflag && ( sbdbt.getSELECTState() | xbee.getSELECTState() ) )
sgrsn 0:a6b6c585a378 130 {
sgrsn 0:a6b6c585a378 131 modechange++;
sgrsn 1:755d8977d4a3 132 if(modechange == 4)modechange = 0;
sgrsn 0:a6b6c585a378 133 modechangeflag = 0;
sgrsn 0:a6b6c585a378 134 wait_ms(500);
sgrsn 0:a6b6c585a378 135 }
sgrsn 1:755d8977d4a3 136 modechangeflag = !( sbdbt.getSELECTState() | xbee.getSELECTState() );
sgrsn 0:a6b6c585a378 137 Maru = sbdbt.getButtonState(maru) | xbee.getButtonState(maru);
sgrsn 0:a6b6c585a378 138 Batu = sbdbt.getButtonState(batu) | xbee.getButtonState(batu);
sgrsn 0:a6b6c585a378 139 Sankaku = (sbdbt.getButtonState(sankaku) | xbee.getButtonState(sankaku)) & modechangeflag;
sgrsn 0:a6b6c585a378 140 Sikaku = (sbdbt.getButtonState(sikaku) | xbee.getButtonState(sikaku)) & modechangeflag;
sgrsn 0:a6b6c585a378 141 l1 = sbdbt.getButtonState(L1) | xbee.getButtonState(L1);
sgrsn 0:a6b6c585a378 142 r1 = sbdbt.getButtonState(R1) | xbee.getButtonState(R1);
sgrsn 0:a6b6c585a378 143 l2 = sbdbt.getButtonState(L2) | xbee.getButtonState(L2);
sgrsn 0:a6b6c585a378 144 r2 = sbdbt.getButtonState(R2) | xbee.getButtonState(R2);
sgrsn 0:a6b6c585a378 145 Ue = sbdbt.getButtonState(ue) | xbee.getButtonState(ue);
sgrsn 0:a6b6c585a378 146 Sita = sbdbt.getButtonState(sita) | xbee.getButtonState(sita);
sgrsn 0:a6b6c585a378 147 Migi = sbdbt.getButtonState(migi) | xbee.getButtonState(migi);
sgrsn 0:a6b6c585a378 148 Hidari = sbdbt.getButtonState(hidari) | xbee.getButtonState(hidari);
sgrsn 0:a6b6c585a378 149 RightY = sbdbt.getRightJoystickYaxis();
sgrsn 0:a6b6c585a378 150 LeftY = sbdbt.getLeftJoystickYaxis();
sgrsn 0:a6b6c585a378 151 LeftX = sbdbt.getLeftJoystickXaxis();
sgrsn 0:a6b6c585a378 152 for(int i = 0; i < 8; i++)
sgrsn 0:a6b6c585a378 153 sensors[i] = (sensor1>>i) & 1;
sgrsn 0:a6b6c585a378 154
sgrsn 0:a6b6c585a378 155 switch(modechange)
sgrsn 0:a6b6c585a378 156 {
sgrsn 0:a6b6c585a378 157 case 0: //start to area1 clear
sgrsn 0:a6b6c585a378 158 /*LeftY : tukamuyatu spin**********************************/
sgrsn 0:a6b6c585a378 159 if(LeftY > 0)
sgrsn 0:a6b6c585a378 160 {
sgrsn 0:a6b6c585a378 161 myi2c.WriteSomeData(MD5_addr, motor1_state, 5, 1);
sgrsn 0:a6b6c585a378 162 }
sgrsn 0:a6b6c585a378 163 else if(LeftY < 0)
sgrsn 0:a6b6c585a378 164 {
sgrsn 0:a6b6c585a378 165 myi2c.WriteSomeData(MD5_addr, motor1_state, 10, 1);
sgrsn 0:a6b6c585a378 166 LeftY = LeftY * (-1);
sgrsn 0:a6b6c585a378 167 }
sgrsn 0:a6b6c585a378 168 else
sgrsn 0:a6b6c585a378 169 {
sgrsn 0:a6b6c585a378 170 myi2c.WriteSomeData(MD5_addr, motor1_state, 3, 1);
sgrsn 0:a6b6c585a378 171 }
sgrsn 0:a6b6c585a378 172 myi2c.WriteSomeData(MD5_addr, motor1_pwm, LeftY * 4-1, 1);
sgrsn 0:a6b6c585a378 173
sgrsn 0:a6b6c585a378 174 /*RightY : rail slide***********************************/
sgrsn 0:a6b6c585a378 175 if(RightY > 0)
sgrsn 0:a6b6c585a378 176 {
sgrsn 0:a6b6c585a378 177 myi2c.WriteSomeData(MD5_addr, motor2_state, 5, 1);
sgrsn 0:a6b6c585a378 178 }
sgrsn 0:a6b6c585a378 179 else if(RightY < 0)
sgrsn 0:a6b6c585a378 180 {
sgrsn 0:a6b6c585a378 181 myi2c.WriteSomeData(MD5_addr, motor2_state, 10, 1);
sgrsn 0:a6b6c585a378 182 RightY = RightY * (-1);
sgrsn 0:a6b6c585a378 183 }
sgrsn 0:a6b6c585a378 184 else
sgrsn 0:a6b6c585a378 185 {
sgrsn 0:a6b6c585a378 186 myi2c.WriteSomeData(MD5_addr, motor2_state, 3, 1);
sgrsn 0:a6b6c585a378 187 }
sgrsn 0:a6b6c585a378 188 myi2c.WriteSomeData(MD5_addr, motor2_pwm, RightY * 4-1, 1);
sgrsn 0:a6b6c585a378 189
sgrsn 0:a6b6c585a378 190 /*Sikaku : cylinder*****************************************/
sgrsn 0:a6b6c585a378 191 myi2c.WriteSomeData(CY1_addr, motor1_state, Sikaku, 1);
sgrsn 0:a6b6c585a378 192 break;
sgrsn 0:a6b6c585a378 193 case 1: //box torikomi to bridge
sgrsn 0:a6b6c585a378 194 /*Sankaku : torikomi taosu********************************************************/
sgrsn 1:755d8977d4a3 195 myi2c.WriteSomeData(MD6_addr, motor2_pwm, 255, 1);
sgrsn 0:a6b6c585a378 196 if(Sankaku == 0)
sgrsn 0:a6b6c585a378 197 {
sgrsn 0:a6b6c585a378 198 sankakuflag = 1;
sgrsn 0:a6b6c585a378 199 }
sgrsn 0:a6b6c585a378 200 if(sankakuflag && Sankaku)
sgrsn 0:a6b6c585a378 201 {
sgrsn 0:a6b6c585a378 202 torikomi_taosu_motor++;
sgrsn 0:a6b6c585a378 203 if((torikomi_taosu_motor % 2))
sgrsn 0:a6b6c585a378 204 {
sgrsn 1:755d8977d4a3 205 myi2c.WriteSomeData(MD6_addr, motor2_state, 5, 1); //torikomi taosu
sgrsn 0:a6b6c585a378 206 }
sgrsn 0:a6b6c585a378 207 else
sgrsn 0:a6b6c585a378 208 {
sgrsn 1:755d8977d4a3 209 myi2c.WriteSomeData(MD6_addr, motor2_state, 10, 1); //torikomi modosu
sgrsn 0:a6b6c585a378 210
sgrsn 0:a6b6c585a378 211 }
sgrsn 0:a6b6c585a378 212 sankakuflag = 0;
sgrsn 0:a6b6c585a378 213 }
sgrsn 0:a6b6c585a378 214 if(sensors[0] == 0)
sgrsn 0:a6b6c585a378 215 {
sgrsn 0:a6b6c585a378 216 sensorsflag[0] = 1;
sgrsn 0:a6b6c585a378 217 }
sgrsn 1:755d8977d4a3 218 if((sensorsflag[0] == 1 && sensors[0] == 1) || !Sankaku)
sgrsn 0:a6b6c585a378 219 {
sgrsn 1:755d8977d4a3 220 myi2c.WriteSomeData(MD6_addr, motor2_state, 3, 1); //torikomi taosi stop
sgrsn 0:a6b6c585a378 221 sensorsflag[0] = 0;
sgrsn 0:a6b6c585a378 222 }
sgrsn 0:a6b6c585a378 223
sgrsn 1:755d8977d4a3 224 /*LeftY : torikomi slide**********************************/
sgrsn 1:755d8977d4a3 225 if(LeftY > 0)
sgrsn 0:a6b6c585a378 226 {
sgrsn 1:755d8977d4a3 227 myi2c.WriteSomeData(MD6_addr, motor1_state, 5, 1);
sgrsn 0:a6b6c585a378 228 }
sgrsn 1:755d8977d4a3 229 else if(LeftY < 0)
sgrsn 0:a6b6c585a378 230 {
sgrsn 1:755d8977d4a3 231 myi2c.WriteSomeData(MD6_addr, motor1_state, 10, 1);
sgrsn 1:755d8977d4a3 232 LeftY = LeftY * (-1);
sgrsn 0:a6b6c585a378 233 }
sgrsn 0:a6b6c585a378 234 else
sgrsn 0:a6b6c585a378 235 {
sgrsn 1:755d8977d4a3 236 myi2c.WriteSomeData(MD6_addr, motor1_state, 3, 1);
sgrsn 0:a6b6c585a378 237 }
sgrsn 1:755d8977d4a3 238 myi2c.WriteSomeData(MD6_addr, motor1_pwm, LeftY * 4-1, 1);
sgrsn 0:a6b6c585a378 239
sgrsn 0:a6b6c585a378 240 /*Sikaku : cylinder*********************************************/
sgrsn 0:a6b6c585a378 241 myi2c.WriteSomeData(CY1_addr, motor1_state, Sikaku, 1);
sgrsn 0:a6b6c585a378 242 break;
sgrsn 0:a6b6c585a378 243 case 2: //bridge to area3
sgrsn 1:755d8977d4a3 244 myi2c.WriteSomeData(SERVO1_addr, motor1_state, (!Sankaku) | (Sankaku << 1) | ((RightY > 60) << 2) | (Sikaku << 3), 1);
sgrsn 1:755d8977d4a3 245 /*RightY : bridge1***********************************************
sgrsn 0:a6b6c585a378 246 if(RightY > 0)
sgrsn 0:a6b6c585a378 247 {
sgrsn 0:a6b6c585a378 248 myi2c.WriteSomeData(MD7_addr, motor1_state, 5, 1);
sgrsn 0:a6b6c585a378 249 }
sgrsn 0:a6b6c585a378 250 else if(RightY < 0)
sgrsn 0:a6b6c585a378 251 {
sgrsn 0:a6b6c585a378 252 myi2c.WriteSomeData(MD7_addr, motor1_state, 10, 1);
sgrsn 0:a6b6c585a378 253 RightY = RightY * (-1);
sgrsn 0:a6b6c585a378 254 }
sgrsn 0:a6b6c585a378 255 else
sgrsn 0:a6b6c585a378 256 {
sgrsn 0:a6b6c585a378 257 myi2c.WriteSomeData(MD7_addr, motor1_state, 3, 1);
sgrsn 0:a6b6c585a378 258 }
sgrsn 0:a6b6c585a378 259 myi2c.WriteSomeData(MD7_addr, motor1_pwm, RightY * 4-1, 1);
sgrsn 0:a6b6c585a378 260
sgrsn 1:755d8977d4a3 261 /*LeftY : bridge2******************************************************
sgrsn 0:a6b6c585a378 262 if(LeftY > 0)
sgrsn 0:a6b6c585a378 263 {
sgrsn 0:a6b6c585a378 264 myi2c.WriteSomeData(MD7_addr, motor2_state, 5, 1);
sgrsn 0:a6b6c585a378 265 }
sgrsn 0:a6b6c585a378 266 else if(LeftY < 0)
sgrsn 0:a6b6c585a378 267 {
sgrsn 0:a6b6c585a378 268 myi2c.WriteSomeData(MD7_addr, motor2_state, 10, 1);
sgrsn 0:a6b6c585a378 269 LeftY = LeftY * (-1);
sgrsn 0:a6b6c585a378 270 }
sgrsn 0:a6b6c585a378 271 else
sgrsn 0:a6b6c585a378 272 {
sgrsn 0:a6b6c585a378 273 myi2c.WriteSomeData(MD7_addr, motor2_state, 3, 1);
sgrsn 0:a6b6c585a378 274 }
sgrsn 1:755d8977d4a3 275 myi2c.WriteSomeData(MD7_addr, motor2_pwm, LeftY * 4-1, 1);*/
sgrsn 0:a6b6c585a378 276 break;
sgrsn 0:a6b6c585a378 277 case 3: //box pile up
sgrsn 0:a6b6c585a378 278 /*LeftX : jack move*************************************************/
sgrsn 1:755d8977d4a3 279 if(!Sankaku)
sgrsn 0:a6b6c585a378 280 {
sgrsn 1:755d8977d4a3 281 if(LeftX > 0)
sgrsn 1:755d8977d4a3 282 {
sgrsn 1:755d8977d4a3 283 myi2c.WriteSomeData(MD8_addr, motor1_state, 5, 1);
sgrsn 1:755d8977d4a3 284 }
sgrsn 1:755d8977d4a3 285 else if(LeftX < 0)
sgrsn 1:755d8977d4a3 286 {
sgrsn 1:755d8977d4a3 287 myi2c.WriteSomeData(MD8_addr, motor1_state, 10, 1);
sgrsn 1:755d8977d4a3 288 LeftX = LeftX * (-1);
sgrsn 1:755d8977d4a3 289 }
sgrsn 1:755d8977d4a3 290 else
sgrsn 1:755d8977d4a3 291 {
sgrsn 1:755d8977d4a3 292 myi2c.WriteSomeData(MD8_addr, motor1_state, 3, 1);
sgrsn 1:755d8977d4a3 293 }
sgrsn 1:755d8977d4a3 294 myi2c.WriteSomeData(MD8_addr, motor1_pwm, LeftX * 4-1, 1);
sgrsn 0:a6b6c585a378 295 }
sgrsn 0:a6b6c585a378 296
sgrsn 0:a6b6c585a378 297 /*Sikaku : jack up***************************************************/
sgrsn 1:755d8977d4a3 298 myi2c.WriteSomeData(MD8_addr, motor2_state, Sikaku * 5, 1);
sgrsn 1:755d8977d4a3 299 myi2c.WriteSomeData(MD8_addr, motor2_pwm, 255, 1);
sgrsn 0:a6b6c585a378 300
sgrsn 0:a6b6c585a378 301 /*RightY : rail up***************************************************/
sgrsn 0:a6b6c585a378 302 if(RightY > 0)
sgrsn 0:a6b6c585a378 303 {
sgrsn 0:a6b6c585a378 304 myi2c.WriteSomeData(MD9_addr, motor1_state, 5, 1);
sgrsn 0:a6b6c585a378 305 myi2c.WriteSomeData(MD9_addr, motor2_state, 5, 1);
sgrsn 0:a6b6c585a378 306 }
sgrsn 0:a6b6c585a378 307 else if(RightY < 0)
sgrsn 0:a6b6c585a378 308 {
sgrsn 0:a6b6c585a378 309 myi2c.WriteSomeData(MD9_addr, motor1_state, 10, 1);
sgrsn 0:a6b6c585a378 310 myi2c.WriteSomeData(MD9_addr, motor2_state, 10, 1);
sgrsn 0:a6b6c585a378 311 RightY = RightY * (-1);
sgrsn 0:a6b6c585a378 312 }
sgrsn 0:a6b6c585a378 313 else
sgrsn 0:a6b6c585a378 314 {
sgrsn 0:a6b6c585a378 315 myi2c.WriteSomeData(MD9_addr, motor1_state, 3, 1);
sgrsn 0:a6b6c585a378 316 myi2c.WriteSomeData(MD9_addr, motor2_state, 3, 1);
sgrsn 0:a6b6c585a378 317 }
sgrsn 0:a6b6c585a378 318 myi2c.WriteSomeData(MD9_addr, motor1_pwm, RightY * 4-1, 1);
sgrsn 0:a6b6c585a378 319 myi2c.WriteSomeData(MD9_addr, motor2_pwm, RightY * 4-1, 1);
sgrsn 0:a6b6c585a378 320
sgrsn 0:a6b6c585a378 321 /*Sankaku & LeftY : center slide**************************************/
sgrsn 0:a6b6c585a378 322 if(Sankaku)
sgrsn 0:a6b6c585a378 323 {
sgrsn 1:755d8977d4a3 324 if(LeftY > 0)
sgrsn 0:a6b6c585a378 325 {
sgrsn 0:a6b6c585a378 326 myi2c.WriteSomeData(MD10_addr, motor1_state, 5, 1);
sgrsn 0:a6b6c585a378 327 }
sgrsn 1:755d8977d4a3 328 else if(LeftY < 0)
sgrsn 0:a6b6c585a378 329 {
sgrsn 0:a6b6c585a378 330 myi2c.WriteSomeData(MD10_addr, motor1_state, 10, 1);
sgrsn 1:755d8977d4a3 331 LeftY = LeftY * (-1);
sgrsn 0:a6b6c585a378 332 }
sgrsn 0:a6b6c585a378 333 else
sgrsn 0:a6b6c585a378 334 {
sgrsn 0:a6b6c585a378 335 myi2c.WriteSomeData(MD10_addr, motor1_state, 3, 1);
sgrsn 0:a6b6c585a378 336 }
sgrsn 1:755d8977d4a3 337 myi2c.WriteSomeData(MD10_addr, motor1_pwm, LeftY * 4-1, 1);
sgrsn 0:a6b6c585a378 338 }
sgrsn 0:a6b6c585a378 339 break;
sgrsn 0:a6b6c585a378 340 }
sgrsn 0:a6b6c585a378 341
sgrsn 0:a6b6c585a378 342 /*Steering******************************************************/
sgrsn 0:a6b6c585a378 343 Dpad_deg = Dpad_table[Migi + 2*Hidari][Ue + 2*Sita];
sgrsn 0:a6b6c585a378 344
sgrsn 0:a6b6c585a378 345 if(Dpad_deg < -90)
sgrsn 0:a6b6c585a378 346 {
sgrsn 0:a6b6c585a378 347 Dpad_deg += 180;
sgrsn 0:a6b6c585a378 348 counterwheel = 1;
sgrsn 0:a6b6c585a378 349 }
sgrsn 0:a6b6c585a378 350 else if(Dpad_deg > 90)
sgrsn 0:a6b6c585a378 351 {
sgrsn 0:a6b6c585a378 352 Dpad_deg -= 180;
sgrsn 0:a6b6c585a378 353 counterwheel = 1;
sgrsn 0:a6b6c585a378 354 }
sgrsn 0:a6b6c585a378 355 else
sgrsn 0:a6b6c585a378 356 {
sgrsn 0:a6b6c585a378 357 counterwheel = 0;
sgrsn 0:a6b6c585a378 358 }
sgrsn 0:a6b6c585a378 359
sgrsn 0:a6b6c585a378 360
sgrsn 0:a6b6c585a378 361 if(Ue | Sita | Migi | Hidari)
sgrsn 0:a6b6c585a378 362 {
sgrsn 0:a6b6c585a378 363 Dpadctrl_flag = 1;
sgrsn 0:a6b6c585a378 364 }
sgrsn 0:a6b6c585a378 365 else
sgrsn 0:a6b6c585a378 366 {
sgrsn 0:a6b6c585a378 367 Dpadctrl_flag = 0;
sgrsn 0:a6b6c585a378 368 }
sgrsn 0:a6b6c585a378 369 if(Batu)
sgrsn 0:a6b6c585a378 370 {
sgrsn 0:a6b6c585a378 371 for(int i = 2; i < 4; i++)
sgrsn 0:a6b6c585a378 372 {
sgrsn 0:a6b6c585a378 373 prev_PM_target[i] = PM_target[i].integer;
sgrsn 0:a6b6c585a378 374 if(Dpadctrl_flag)
sgrsn 0:a6b6c585a378 375 {
sgrsn 0:a6b6c585a378 376 //PM_target[i].integer = (int)((Dpad_deg > 0)? Dpad_deg + 0.5 : Dpad_deg - 0.5) + PM_ofset[i].integer;
sgrsn 0:a6b6c585a378 377 PM_target[i].integer = Dpad_deg + PM_ofset[i].integer;
sgrsn 0:a6b6c585a378 378 }
sgrsn 0:a6b6c585a378 379 RE_target[i].integer = 300 * Maru;
sgrsn 0:a6b6c585a378 380 }
sgrsn 0:a6b6c585a378 381 }
sgrsn 0:a6b6c585a378 382 else
sgrsn 0:a6b6c585a378 383 {
sgrsn 0:a6b6c585a378 384 for(int i = 0; i < 4; i++)
sgrsn 0:a6b6c585a378 385 {
sgrsn 0:a6b6c585a378 386 prev_PM_target[i] = PM_target[i].integer;
sgrsn 0:a6b6c585a378 387 if(Dpadctrl_flag)
sgrsn 0:a6b6c585a378 388 {
sgrsn 0:a6b6c585a378 389 //PM_target[i].integer = (int)((Dpad_deg > 0)? Dpad_deg + 0.5 : Dpad_deg - 0.5) + PM_ofset[i].integer;
sgrsn 0:a6b6c585a378 390 PM_target[i].integer = Dpad_deg + PM_ofset[i].integer;
sgrsn 0:a6b6c585a378 391 }
sgrsn 0:a6b6c585a378 392 RE_target[i].integer = 300 * Maru;
sgrsn 0:a6b6c585a378 393 }
sgrsn 0:a6b6c585a378 394 }
sgrsn 0:a6b6c585a378 395 if(Maru)
sgrsn 0:a6b6c585a378 396 {
sgrsn 0:a6b6c585a378 397 wheel[0] = 2 - counterwheel;
sgrsn 0:a6b6c585a378 398 wheel[1] = 1 + counterwheel;
sgrsn 0:a6b6c585a378 399 wheel[2] = 1 + counterwheel;
sgrsn 0:a6b6c585a378 400 wheel[3] = 2 - counterwheel;
sgrsn 0:a6b6c585a378 401
sgrsn 0:a6b6c585a378 402 if(l2)
sgrsn 0:a6b6c585a378 403 {
sgrsn 0:a6b6c585a378 404 wheel[0] = 1;
sgrsn 0:a6b6c585a378 405 wheel[1] = 1;
sgrsn 0:a6b6c585a378 406 wheel[2] = 1;
sgrsn 0:a6b6c585a378 407 wheel[3] = 1;
sgrsn 0:a6b6c585a378 408 }
sgrsn 0:a6b6c585a378 409 if(r2)
sgrsn 0:a6b6c585a378 410 {
sgrsn 0:a6b6c585a378 411 wheel[0] = 2;
sgrsn 0:a6b6c585a378 412 wheel[1] = 2;
sgrsn 0:a6b6c585a378 413 wheel[2] = 2;
sgrsn 0:a6b6c585a378 414 wheel[3] = 2;
sgrsn 0:a6b6c585a378 415 }
sgrsn 0:a6b6c585a378 416 }
sgrsn 0:a6b6c585a378 417 /*else if(Batu)
sgrsn 0:a6b6c585a378 418 {
sgrsn 0:a6b6c585a378 419 wheel[0] = 1 + counterwheel;
sgrsn 0:a6b6c585a378 420 wheel[1] = 2 - counterwheel;
sgrsn 0:a6b6c585a378 421 wheel[2] = 2 - counterwheel;
sgrsn 0:a6b6c585a378 422 wheel[3] = 1 + counterwheel;
sgrsn 0:a6b6c585a378 423 }*/
sgrsn 0:a6b6c585a378 424 else
sgrsn 0:a6b6c585a378 425 {
sgrsn 0:a6b6c585a378 426 wheel[0] = 0;
sgrsn 0:a6b6c585a378 427 wheel[1] = 0;
sgrsn 0:a6b6c585a378 428 wheel[2] = 0;
sgrsn 0:a6b6c585a378 429 wheel[3] = 0;
sgrsn 0:a6b6c585a378 430 }
sgrsn 0:a6b6c585a378 431 if(r1)
sgrsn 0:a6b6c585a378 432 {
sgrsn 0:a6b6c585a378 433 if(Batu)
sgrsn 0:a6b6c585a378 434 {
sgrsn 0:a6b6c585a378 435 for(int i = 2; i < 4; i++)
sgrsn 0:a6b6c585a378 436 PM_target[i].integer += 5;
sgrsn 0:a6b6c585a378 437 Dpadctrl_flag = 0;
sgrsn 0:a6b6c585a378 438 }
sgrsn 0:a6b6c585a378 439 else
sgrsn 0:a6b6c585a378 440 {
sgrsn 0:a6b6c585a378 441 for(int i = 0; i < 4; i++)
sgrsn 0:a6b6c585a378 442 PM_target[i].integer += 5;
sgrsn 0:a6b6c585a378 443 Dpadctrl_flag = 0;
sgrsn 0:a6b6c585a378 444 }
sgrsn 0:a6b6c585a378 445 }
sgrsn 0:a6b6c585a378 446 else if(l1)
sgrsn 0:a6b6c585a378 447 {
sgrsn 0:a6b6c585a378 448 if(Batu)
sgrsn 0:a6b6c585a378 449 {
sgrsn 0:a6b6c585a378 450 for(int i = 2; i < 4; i++)
sgrsn 0:a6b6c585a378 451 PM_target[i].integer -= 5;
sgrsn 0:a6b6c585a378 452 Dpadctrl_flag = 0;
sgrsn 0:a6b6c585a378 453 }
sgrsn 0:a6b6c585a378 454 else
sgrsn 0:a6b6c585a378 455 {
sgrsn 0:a6b6c585a378 456 for(int i = 0; i < 4; i++)
sgrsn 0:a6b6c585a378 457 PM_target[i].integer -= 5;
sgrsn 0:a6b6c585a378 458 Dpadctrl_flag = 0;
sgrsn 0:a6b6c585a378 459 }
sgrsn 0:a6b6c585a378 460 }
sgrsn 0:a6b6c585a378 461 else if(r2 | l2)
sgrsn 0:a6b6c585a378 462 {
sgrsn 0:a6b6c585a378 463 PM_target[0].integer = 45 + PM_ofset[0].integer;
sgrsn 0:a6b6c585a378 464 PM_target[1].integer = -45 + PM_ofset[1].integer;
sgrsn 0:a6b6c585a378 465 PM_target[2].integer = 45 + PM_ofset[2].integer;
sgrsn 0:a6b6c585a378 466 PM_target[3].integer = -45 + PM_ofset[3].integer;
sgrsn 0:a6b6c585a378 467 Dpadctrl_flag = 0;
sgrsn 0:a6b6c585a378 468 }
sgrsn 0:a6b6c585a378 469 /*wheel communication*************************************/
sgrsn 0:a6b6c585a378 470
sgrsn 0:a6b6c585a378 471 for(int address = 0; address < 4; address++)
sgrsn 0:a6b6c585a378 472 {
sgrsn 0:a6b6c585a378 473 myi2c.WriteSomeData(MD1_addr + (0x02*address), wheel_start, wheel[address], 1);
sgrsn 0:a6b6c585a378 474 if(PM_target[address].integer != prev_PM_target[address])
sgrsn 0:a6b6c585a378 475 {
sgrsn 0:a6b6c585a378 476 myi2c.WriteSomeData(MD1_addr + (0x02*address), PM_start, 0, 1);
sgrsn 0:a6b6c585a378 477 }
sgrsn 0:a6b6c585a378 478
sgrsn 0:a6b6c585a378 479 for(int i = 0; i < 4; i++)
sgrsn 0:a6b6c585a378 480 {
sgrsn 0:a6b6c585a378 481 myi2c.WriteSomeData(MD1_addr + (0x02*address), PM_target_reg + i, PM_target[address].character[i], 1);
sgrsn 0:a6b6c585a378 482 myi2c.WriteSomeData(MD1_addr + (0x02*address), RE_target_reg + i, RE_target[address].character[i], 1);
sgrsn 0:a6b6c585a378 483 }
sgrsn 0:a6b6c585a378 484 //myi2c.WriteSomeData(MD1_addr + (0x02*address), pid_start, 1, 1);
sgrsn 0:a6b6c585a378 485 myi2c.WriteSomeData(MD1_addr + (0x02*address), PM_start, 1, 1);
sgrsn 0:a6b6c585a378 486 }
sgrsn 0:a6b6c585a378 487 myi2c.getSlaveRegistarData(SE1_addr, 0, &sensor1, 1);
sgrsn 0:a6b6c585a378 488 wait_ms(20);
sgrsn 0:a6b6c585a378 489 myi2c.getSlaveRegistarData(SE2_addr, 0, &sensor2, 1);
sgrsn 0:a6b6c585a378 490 wait_ms(20);
sgrsn 0:a6b6c585a378 491 }
sgrsn 0:a6b6c585a378 492 }
sgrsn 0:a6b6c585a378 493
sgrsn 0:a6b6c585a378 494 void getpc()
sgrsn 0:a6b6c585a378 495 {
sgrsn 0:a6b6c585a378 496 if(pc.readable())
sgrsn 0:a6b6c585a378 497 {
sgrsn 0:a6b6c585a378 498 prev_pcdata = pcdata;
sgrsn 0:a6b6c585a378 499 pcdata = pc.getc();
sgrsn 0:a6b6c585a378 500 }
sgrsn 0:a6b6c585a378 501 }