robocon_2016 / Mbed 2 deprecated 2016_mother_3

mother

Dependencies:   PS3 i2cmaster mbed

main.cpp@0:a6b6c585a378, 2016-09-26 (annotated)

Committer:
sgrsn
Date:
Mon Sep 26 10:29:21 2016 +0000
Revision:
0:a6b6c585a378
Child:
1:755d8977d4a3
mother

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