unsigned char PS2X::_gamepad_shiftinout (char byte) { SaveInterruptState; // *** KJE *** save away the current state of interrupts unsigned char tmp = 0; DisableInterrupts; // *** KJE *** disable for now for(i=0;i<8;i++) { if(CHK(byte,i)) CMD_SET(); else CMD_CLR(); CLK_CLR(); RestoreInterrupts; // *** *** KJE *** *** Interrupts may be enabled again delayMicroseconds(CTRL_CLK); DisableInterrupts; // *** KJE *** if(DAT_CHK()) SET(tmp,i); CLK_SET(); #if CTRL_CLK_HIGH delayMicroseconds(CTRL_CLK_HIGH); #endif } CMD_SET(); RestoreInterrupts; // *** *** KJE *** *** Interrupts may be enabled again delayMicroseconds(CTRL_BYTE_DELAY); return tmp; }
unsigned char PS2X::_gamepad_shiftinout (char byte) { unsigned char tmp = 0; for(unsigned char i=0;i<8;i++) { if(CHK(byte,i)) CMD_SET(); else CMD_CLR(); CLK_CLR(); delayMicroseconds(CTRL_CLK); //if(DAT_CHK()) SET(tmp,i); if(DAT_CHK()) bitSet(tmp,i); CLK_SET(); #if CTRL_CLK_HIGH delayMicroseconds(CTRL_CLK_HIGH); #endif } CMD_SET(); delayMicroseconds(CTRL_BYTE_DELAY); return tmp; }
int VCSetDACVolume(Command_t * command, cmd_dac_gain_id dac_gain_id) { int ret_val, mem_info; reset_time_out(); mem_info = dac_gain_id; command->data[0] = 0; command->data[1] = 0; command->data[2] = 1; command->data[3] = mkword(0x34, mem_info); ret_val = send_cmd(command, APP_ID_CTRL, CMD_SET(CONTROL_APP_I2C_TUNNEL_DATA), 4); if (ret_val<0) { return ret_val; /* err code*/ } command->data[0] = 0; ret_val = send_cmd(command, APP_ID_CTRL, CONTROL_APP_I2C_TUNNEL_APPLY, 1); if (ret_val<0) { return ret_val; /* err code*/ } mem_info |= 0x100; command->data[0] = 0; command->data[1] = 0; command->data[2] = 1; command->data[3] = mkword(0x35, mem_info); ret_val = send_cmd(command, APP_ID_CTRL, CMD_SET(CONTROL_APP_I2C_TUNNEL_DATA), 4); if (ret_val<0) { return ret_val; /* err code*/ } command->data[0] = 0; ret_val = send_cmd(command, APP_ID_CTRL, CONTROL_APP_I2C_TUNNEL_APPLY, 1); if (ret_val<0) { return ret_val; /* err code*/ } return(0); }
// 复位操作耗时1s int VCReset(Command_t * command) { int ntry = 3; int32_t ret; // reset on release version 4.1010 command->data[0] = IV_I2C_CMD_GET_VERSION; ret = send_cmd(command, APP_ID_CAPT | capt_nr, (IV_I2C_CMD_CTRL | 0x0100), 1); printk("VCReset:ret=%d, %d, %d\n", ret, command->data[0], command->data[1]); if(ret < 0) return 0; // if (command->data[0] != 4 && command->data[1] != 0x10100) return 0; if (!(command->data[0] == 4 && command->data[1] == 0x10100)) return 0; while (ntry-- > 0) { //check watchdog ret = send_cmd(command, APP_ID_CAPT|capt_nr, CMD_GET(IVNR_CMD_WATCHDOG), 0); printk("get watchdog:%d %d\n", ret, command->data[0]); if( ret < 0 ) continue; if (command->data[0] == 0) { // start watchdog command->data[0] = 1; ret = send_cmd(command, APP_ID_CAPT|capt_nr, CMD_SET(IVNR_CMD_WATCHDOG), 1); printk("set watchdog:%d %d\n", ret, command->data[0]); if( ret < 0 ) continue; } // close app command->data[0] = APP_ID_CAPT; command->data[1] = 0; // ret = send_cmd(command, APP_ID_STRM, STREAMER_APP_MODULE_APP_ENABLE, 2); if (ret < 0) continue; mdelay(1000);// 复位后要等1s才能操作净音模块,这里用了阻塞式等待 //check watchdog ret = send_cmd(command, APP_ID_CAPT|capt_nr, CMD_GET(IVNR_CMD_WATCHDOG), 0); printk("get watchdog:%d %d\n", ret, command->data[0]); if( ret < 0 ) continue; if (command->data[0] == 1) break; } return 0; }
void Robot::Packet(move_cmd_t * cmd, uint8_t speed_m1, uint8_t speed_m2){ if(speed_m1 > 0) cmd->action1 = CMD_SET(FORWARD) | MOTOR_SET(1); else if(speed_m1 < 0) cmd->action1 = CMD_SET(REVERSE) | MOTOR_SET(1); else cmd->action1 = CMD_SET(BRAKE1) | MOTOR_SET(1); cmd->speed1 = Abs(speed_m1); if(speed_m2 > 0) cmd->action2 = CMD_SET(FORWARD) | MOTOR_SET(2); else if(speed_m2 < 0) cmd->action2 = CMD_SET(REVERSE) | MOTOR_SET(2); else cmd->action2 = CMD_SET(BRAKE1) | MOTOR_SET(2); cmd->speed2 = Abs(speed_m2); }
void PS2X::read_gamepad(boolean motor1, byte motor2) { double temp = millis() - last_read; SaveInterruptState; // *** KJE **** save away the current state of interrupts - *** *** KJE *** *** if (temp > 1500) //waited to long reconfig_gamepad(); if(temp < read_delay) //waited too short delay(read_delay - temp); last_buttons = buttons; //store the previous buttons states if(motor2 != 0x00) motor2 = map(motor2,0,255,0x40,0xFF); //noting below 40 will make it spin DisableInterrupts; //*** KJE *** CMD_SET(); CLK_SET(); ATT_CLR(); // low enable joystick RestoreInterrupts; // *** KJE *** - Interrupts may be enabled again delayMicroseconds(CTRL_BYTE_DELAY); //Send the command to send button and joystick data; char dword[9] = {0x01,0x42,0,motor1,motor2,0,0,0,0}; byte dword2[12] = {0,0,0,0,0,0,0,0,0,0,0,0}; for (int i = 0; i<9; i++) { PS2data[i] = _gamepad_shiftinout(dword[i]); } if(PS2data[1] == 0x79) { //if controller is in full data return mode, get the rest of data for (int i = 0; i<12; i++) { PS2data[i+9] = _gamepad_shiftinout(dword2[i]); } } DisableInterrupts; ATT_SET(); // HI disable joystick RestoreInterrupts; // Interrupts may be enabled again #ifdef PS2X_COM_DEBUG Serial.println("OUT:IN"); for(int i=0; i<9; i++){ Serial.print(dword[i], HEX); Serial.print(":"); Serial.print(PS2data[i], HEX); Serial.print(" "); } for (int i = 0; i<12; i++) { Serial.print(dword2[i], HEX); Serial.print(":"); Serial.print(PS2data[i+9], HEX); Serial.print(" "); } Serial.println(""); #endif //buttons = *(uint16_t*)(PS2data+3); // FAULTS on chipkit!!! store as one value for multiple functions buttons = (uint16_t)(PS2data[4] << 8) + PS2data[3]; //store as one value for multiple functions last_read = millis(); }
byte PS2X::config_gamepad(uint8_t clk, uint8_t cmd, uint8_t att, uint8_t dat, bool pressures, bool rumble) { SaveInterruptState; // *** KJE *** save away the current state of interrupts byte temp[sizeof(type_read)]; #ifdef __AVR__ _clk_mask = maskToBitNum(digitalPinToBitMask(clk)); _clk_oreg = portOutputRegister(digitalPinToPort(clk)); _cmd_mask = Num(digitalPinToBitMask(cmd)); _cmd_oreg = portOutputRegister(digitalPinToPort(cmd)); _att_mask = maskToBitNum(digitalPinToBitMask(att)); _att_oreg = portOutputRegister(digitalPinToPort(att)); _dat_mask = maskToBitNum(digitalPinToBitMask(dat)); _dat_ireg = portInputRegister(digitalPinToPort(dat)); #else _clk_mask = digitalPinToBitMask(clk); _clk_lport = portOutputRegister(digitalPinToPort(clk)); _cmd_mask = digitalPinToBitMask(cmd); _cmd_lport = portOutputRegister(digitalPinToPort(cmd)); _att_mask = digitalPinToBitMask(att); _att_lport = portOutputRegister(digitalPinToPort(att)); _dat_mask = digitalPinToBitMask(dat); _dat_lport = portInputRegister(digitalPinToPort(dat)); #endif pinMode(clk, OUTPUT); //configure ports pinMode(att, OUTPUT); pinMode(cmd, OUTPUT); pinMode(dat, INPUT); #if defined(__AVR__) digitalWrite(dat, HIGH); //enable pull-up #endif DisableInterrupts; // *** KJE *** disable for now CMD_SET(); // SET(*_cmd_oreg,_cmd_mask); CLK_SET(); RestoreInterrupts; // *** *** KJE *** *** Interrupts may be enabled again //new error checking. First, read gamepad a few times to see if it's talking read_gamepad(); read_gamepad(); //see if it talked if(PS2data[1] != 0x41 && PS2data[1] != 0x73 && PS2data[1] != 0x79){ //see if mode came back. If still anything but 41, 73 or 79, then it's not talking #ifdef PS2X_DEBUG Serial.println("Controller mode not matched or no controller found"); Serial.print("Expected 0x41 or 0x73, got "); Serial.println(PS2data[1], HEX); #endif return 1; //return error code 1 } //try setting mode, increasing delays if need be. read_delay = 1; for(int y = 0; y <= 10; y++) { sendCommandString(enter_config, sizeof(enter_config)); //start config run //read type delayMicroseconds(CTRL_BYTE_DELAY); DisableInterrupts; // *** KJE *** disable for now CMD_SET(); CLK_SET(); ATT_CLR(); // low enable joystick RestoreInterrupts; // *** *** KJE *** *** Interrupts may be enabled again delayMicroseconds(CTRL_BYTE_DELAY); for (int i = 0; i<9; i++) { temp[i] = _gamepad_shiftinout(type_read[i]); } DisableInterrupts; // *** KJE *** disable for now ATT_SET(); // HI disable joystick RestoreInterrupts; // *** *** KJE *** *** Interrupts may be enabled again controller_type = temp[3]; sendCommandString(set_mode, sizeof(set_mode)); if(rumble){ sendCommandString(enable_rumble, sizeof(enable_rumble)); en_Rumble = true; } if(pressures){ sendCommandString(set_bytes_large, sizeof(set_bytes_large)); en_Pressures = true; } sendCommandString(exit_config, sizeof(exit_config)); read_gamepad(); if(pressures){ if(PS2data[1] == 0x79) break; if(PS2data[1] == 0x73) return 3; } if(PS2data[1] == 0x73) break; if(y == 10){ #ifdef PS2X_DEBUG Serial.println("Controller not accepting commands"); Serial.print("mode stil set at"); Serial.println(PS2data[1], HEX); #endif return 2; //exit function with error } read_delay += 1; //add 1ms to read_delay } return 0; //no error if here }
boolean PS2X::read_gamepad(boolean motor1, byte motor2) { double temp = millis() - last_read; if (temp > 1500) //waited to long reconfig_gamepad(); if(temp < read_delay) //waited too short delay(read_delay - temp); if(motor2 != 0x00) motor2 = map(motor2,0,255,0x40,0xFF); //noting below 40 will make it spin char dword[9] = {0x01,0x42,0,motor1,motor2,0,0,0,0}; byte dword2[12] = {0,0,0,0,0,0,0,0,0,0,0,0}; // Try a few times to get valid data... for (byte RetryCnt = 0; RetryCnt < 5; RetryCnt++) { CMD_SET(); CLK_SET(); ATT_CLR(); // low enable joystick delayMicroseconds(CTRL_BYTE_DELAY); //Send the command to send button and joystick data; for (int i = 0; i<9; i++) { PS2data[i] = _gamepad_shiftinout(dword[i]); } if(PS2data[1] == 0x79) { //if controller is in full data return mode, get the rest of data for (int i = 0; i<12; i++) { PS2data[i+9] = _gamepad_shiftinout(dword2[i]); } } ATT_SET(); // HI disable joystick // Check to see if we received valid data or not. // We should be in analog mode for our data to be valid (analog == 0x7_) if ((PS2data[1] & 0xf0) == 0x70) break; // If we got to here, we are not in analog mode, try to recover... reconfig_gamepad(); // try to get back into Analog mode. delay(read_delay); } // If we get here and still not in analog mode (=0x7_), try increasing the read_delay... if ((PS2data[1] & 0xf0) != 0x70) { if (read_delay < 10) read_delay++; // see if this helps out... } #ifdef PS2X_COM_DEBUG Serial.println("OUT:IN"); for(int i=0; i<9; i++){ Serial.print(dword[i], HEX); Serial.print(":"); Serial.print(PS2data[i], HEX); Serial.print(" "); } for (int i = 0; i<12; i++) { Serial.print(dword2[i], HEX); Serial.print(":"); Serial.print(PS2data[i+9], HEX); Serial.print(" "); } Serial.println(""); #endif last_buttons = buttons; //store the previous buttons states #if defined(__AVR__) buttons = *(uint16_t*)(PS2data+3); //store as one value for multiple functions #else buttons = (uint16_t)(PS2data[4] << 8) + PS2data[3]; //store as one value for multiple functions #endif last_read = millis(); return ((PS2data[1] & 0xf0) == 0x70); // 1 = OK = analog mode - 0 = NOK }
byte PS2X::config_gamepad(uint8_t clk, uint8_t cmd, uint8_t att, uint8_t dat, bool pressures, bool rumble) { byte temp[sizeof(type_read)]; _clk_mask = digitalPinToBitMask(clk); _clk_oreg = portOutputRegister(digitalPinToPort(clk)); _cmd_mask = digitalPinToBitMask(cmd); _cmd_oreg = portOutputRegister(digitalPinToPort(cmd)); _att_mask = digitalPinToBitMask(att); _att_oreg = portOutputRegister(digitalPinToPort(att)); _dat_mask = digitalPinToBitMask(dat); _dat_ireg = portInputRegister(digitalPinToPort(dat)); MC_SET_PIN_OUTPUT(PS2_PIN_CLK); MC_SET_PIN_OUTPUT(PS2_PIN_CMD); MC_SET_PIN_OUTPUT(PS2_PIN_SEL); MC_SET_PIN_INPUT(PS2_PIN_DAT); CMD_SET(); // SET(*_cmd_oreg,_cmd_mask); CLK_SET(); //new error checking. First, read gamepad a few times to see if it's talking read_gamepad(); read_gamepad(); //see if it talked - see if mode came back. //If still anything but 41, 73 or 79, then it's not talking if(PS2data[1] != 0x41 && PS2data[1] != 0x73 && PS2data[1] != 0x79) { return 1; //return error code 1 } //try setting mode, increasing delays if need be. read_delay = 1; for(int y = 0; y <= 10; y++) { sendCommandString(enter_config, sizeof(enter_config)); //start config run //read type _delay_us(CTRL_BYTE_DELAY); CMD_SET(); CLK_SET(); ATT_CLR(); // low enable joystick _delay_us(CTRL_BYTE_DELAY); for (int i = 0; i<9; i++) { temp[i] = _gamepad_shiftinout(type_read[i]); } ATT_SET(); // HI disable joystick controller_type = temp[3]; sendCommandString(set_mode, sizeof(set_mode)); sendCommandString(exit_config, sizeof(exit_config)); read_gamepad(); if(pressures) { if(PS2data[1] == 0x79) { break; } if(PS2data[1] == 0x73) { return 3; } } if(PS2data[1] == 0x73) { break; } if(y == 10) { return 2; //exit function with error } read_delay += 1; //add 1ms to read_delay } return 0; //no error if here }