void reset() { state = State.NotInitialized; _timeout = 0f; _state = machineState.None; _deviceAddress = null; _foundAccel = false; _foundGyro = false; _foundTouch = false; }
public ContextSwitchValue(IEqualityComparer <stateType> StateComparer, IEqualityComparer <actionType> ActionComparer, List <actionType> AvailableActions, stateType StartState, params object[] parameters) : base(StateComparer, ActionComparer, AvailableActions, StartState, parameters) { if (parameters.Length > 0) { layers = (int)parameters[0]; } if (parameters.Length > 1) { maxUpdates = (int)parameters[1]; } stateComparer = StateComparer; actionComparer = ActionComparer; availableActions = AvailableActions; startState = StartState; currentModel = newModel(defaultQValue); models.Add(currentModel); currentMachineState = machineState.useCurrent; }
public override double[] value(stateType state, List <actionType> actions) { if (currentModel == null) { double bestValue = double.NegativeInfinity; foreach (MultiResValue <stateType, actionType> m in models) { double thisValue = m.models[0].value((int[])(object)state, actions).Max(); if (thisValue > bestValue) { bestValue = thisValue; currentModel = m; } } Console.WriteLine("starting with model " + models.IndexOf(currentModel)); } if (currentMachineState == machineState.tryAdapt) { try { return(currentModel.value((int[])((object)state), actions)); } catch (ApplicationException ex) { //candidateModel = null; //currentModel = newModel(15); //models.Add(currentModel); currentModel = candidateModel; candidateModel = null; models.Add(currentModel); currentModel.models[0].defaultQ = 15; currentMachineState = machineState.useCurrent; Console.WriteLine("Starting new model (" + models.Count + ")"); return(currentModel.value((int[])((object)state), actions)); } } else if (currentMachineState == machineState.useCurrent) { return(currentModel.models[0].value((int[])((object)state), actions)); } return(null); // // switch to the model which best explains the recent transition history // double bestP = 0; // MultiResValue<stateType, actionType> bestModel = currentModel; // foreach (MultiResValue<stateType, actionType> m in models) // { // if (m == currentModel) // continue; // double thisP = EventProbability(transitionHistory, m, priorCnts); //Console.WriteLine(thisP); //if (thisP > (bestP + 0.05)) // { // bestP = thisP; // bestModel = m; // if (thisP >= pThreshold) // { // Console.WriteLine("Switching to previously learned model: " + models.IndexOf(m) + "(p = " + Math.Round(thisP, 2) + ")"); // currentModel = m; // } // } // } // if (bestP < pThreshold) // if none explain it well // { // currentModel = newModel(15); // models.Add(currentModel); // Console.WriteLine("Starting new model (p = " + bestP + ")"); // } // currentMachineState = machineState.useCurrent; // return currentModel.value((int[])((object)state), actions); //} // break; //} //return null; }
public override double update(StateTransition <stateType, actionType> transition) { transitionHistory.Enqueue(transition); while (transitionHistory.Count() > 100) { transitionHistory.Dequeue(); } switch (currentMachineState) { case machineState.useCurrent: //// switch to the model which best explains the recent transition history //double bestP = EventProbability(transitionHistory, currentModel, priorCnts); ////Console.WriteLine(bestP); //MultiResValue < stateType, actionType > bestModel = currentModel; //foreach (MultiResValue<stateType, actionType> m in models) //{ // if (m == currentModel) // continue; // double thisP = EventProbability(transitionHistory, m, priorCnts); // if (thisP > (bestP + 0.05)) // { // if (thisP >= pThreshold) // { // Console.WriteLine("Switching to previously learned model: " + models.IndexOf(m) + "(p = " + Math.Round(thisP, 2) + " vs " + Math.Round(bestP,2) + ")"); // currentModel = m; // } // bestP = thisP; // bestModel = m; // } //} ////Console.WriteLine(bestP); double[] pVals = new double[models.Count]; double currentVal = double.NaN; for (int i = 0; i < models.Count; i++) { pVals[i] = EventProbability(transitionHistory, models[i], priorCnts); if (models[i] == currentModel) { currentVal = pVals[i]; } } int bestModelIndex = softmax(pVals); if (pVals[bestModelIndex] > (currentVal + 0.05)) { softmax(pVals); currentModel = models[bestModelIndex]; Console.WriteLine("Switching to previously learned model: " + bestModelIndex + "(p = " + Math.Round(pVals[bestModelIndex], 2) + ")"); } if (pVals.Max() < pThreshold) // if none explain it well { //// find the model with the best value from the current state //double bestVal = currentModel.models[0].value((int[])((object)transition.newState), availableActions).Max(); //bestModel = currentModel; //foreach (MultiResValue<stateType, actionType> m in models) //{ // double thisVal = m.models[0].value((int[])((object)transition.newState), availableActions).Max(); // if (thisVal > bestVal) // { // bestVal = thisVal; // bestModel = m; // } //} // does the unexpected event relate to reward? double rProb = currentModel.models[0].PredictReward((int[])(object)transition.oldState, transition.action).P(transition.reward, 1); //Console.WriteLine(rProb); bool rewardRelatedError = rProb < pThreshold; if (layers > 1 && !rewardRelatedError) { // copy the best model for adaptation Console.WriteLine("Adapting model " + bestModelIndex + " (p = " + pVals[bestModelIndex] + ")"); // + ", bestVal = " + bestVal + ")"); currentModel = copyModel(models[bestModelIndex]); //models.Add(currentModel); //??????????????????? if not here then move to adaptation successful candidateModel = newModel(0.001); currentMachineState = machineState.tryAdapt; } else { currentModel = newModel(defaultQValue); resetHistory(); models.Add(currentModel); Console.WriteLine("Starting new model (p = " + pVals[bestModelIndex] + ")(" + models.Count + ")"); } } break; case machineState.tryAdapt: // let the candidate model see the state transition candidateModel.update((StateTransition <int[], actionType>)((object)transition)); // if goal has been found, assume model is adapted successfully if (transition.reward > 0) { currentMachineState = machineState.useCurrent; currentModel = candidateModel; models.Add(candidateModel); //models.Add(currentModel); Console.WriteLine("Adaptation successful"); break; } //// switch to the model which best explains the recent transition history //bestP = EventProbability(transitionHistory, currentModel, 1); //bestModel = currentModel; //foreach (MultiResValue<stateType, actionType> m in models) //{ // if (m == currentModel) // continue; // double thisP = EventProbability(transitionHistory, m, 1); // if (thisP > (bestP + 0.05)) // { // bestP = thisP; // bestModel = m; // if (thisP >= pThreshold) // { // Console.WriteLine("Adaptation aborted. Switching to previously learned model: " + models.IndexOf(m) + "(p = " + Math.Round(thisP, 2) + ")"); // currentModel = m; // candidateModel = null; // currentMachineState = machineState.useCurrent; // } // } //} // if value gradient flattens, assume model cannot be adapted double currentValue = currentModel.models[0].value((int[])((object)transition.newState), availableActions).Max(); if (currentValue < vThreshold) { //currentModel = newModel(15); //models.Add(currentModel); //candidateModel = null; currentModel = candidateModel; candidateModel = null; models.Add(currentModel); currentModel.models[0].defaultQ = 15; currentMachineState = machineState.useCurrent; Console.WriteLine("Adaptation failed. Starting new model (" + models.Count + ")"); //// switch to the model which best explains the recent transition history //bestP = 0; //bestModel = currentModel; //foreach (MultiResValue<stateType, actionType> m in models) //{ // if (m == currentModel) // continue; // double thisP = EventProbability(transitionHistory, m, priorCnts); // Console.WriteLine(thisP); // if (thisP > (bestP + 0.05)) // { // bestP = thisP; // bestModel = m; // if (thisP >= pThreshold) // { // Console.WriteLine("Switching to previously learned model: " + models.IndexOf(m) + "(p = " + Math.Round(thisP, 2) + ")"); // currentModel = m; // } // } //} //if (bestP < pThreshold) // if none explain it well //{ // currentModel = newModel(15); // models.Add(currentModel); // Console.WriteLine("Starting new model (p = " + bestP + ")"); //} //currentMachineState = machineState.useCurrent; } break; } currentModel.update((StateTransition <int[], actionType>)((object)transition)); if (transition.absorbingStateReached) { resetHistory(); } return(0); }
// Update is called once per frame void Update() { if (_timeout > 0f) { _timeout -= Time.deltaTime; if (_timeout <= 0f) { _timeout = 0f; switch (_state) { case machineState.None: break; case machineState.Scan: BluetoothLEHardwareInterface.ScanForPeripheralsWithServices(null, (address, name) => { // if your device does not advertise the rssi and manufacturer specific data // then you must use this callback because the next callback only gets called // if you have manufacturer specific data Debug.Log("Found Device : " + address + " / Name : " + name); if (name.Contains(DeviceName)) { BluetoothLEHardwareInterface.StopScan(); state = State.DeviceFound; device_name = name; // found a device with the name we want // this example does not deal with finding more than one _deviceAddress = address; SetState(machineState.Connect, 0.5f); } }, (address, name, rssi, bytes) => { // APIdou does not broadcast manufacturer specific data } ); break; case machineState.Connect: // set these flags _foundAccel = false; _foundGyro = false; _foundTouch = false; // note that the first parameter is the address, not the name. I have not fixed this because // of backwards compatiblity. // also note that I am note using the first 2 callbacks. If you are not looking for specific characteristics you can use one of // the first 2, but keep in mind that the device will enumerate everything and so you will want to have a timeout // large enough that it will be finished enumerating before you try to subscribe or do any other operations. BluetoothLEHardwareInterface.ConnectToPeripheral(_deviceAddress, null, null, (address, foundServiceUUID, foundCharUUID) => { if (IsEqual(foundServiceUUID, ServiceUUID)) { _foundAccel = _foundAccel || IsEqual(foundCharUUID, AccelUUID); _foundGyro = _foundGyro || IsEqual(foundCharUUID, GyroUUID); _foundTouch = _foundTouch || IsEqual(foundCharUUID, TouchUUID); // if we have found all characteristics that we are waiting for // set the state. make sure there is enough timeout that if the // device is still enumerating other characteristics it finishes // before we try to subscribe if (_foundAccel && _foundGyro && _foundTouch) { Debug.Log("Connected"); state = State.Connected; SetState(machineState.Subscribe, 3f); } } }); break; case machineState.Subscribe: BluetoothLEHardwareInterface.SubscribeCharacteristicWithDeviceAddress(_deviceAddress, FullUUID(ServiceUUID), FullUUID(TouchUUID), null, onNotification); System.Threading.Thread.Sleep(500); BluetoothLEHardwareInterface.SubscribeCharacteristicWithDeviceAddress(_deviceAddress, FullUUID(ServiceUUID), FullUUID(AccelUUID), null, onNotification); System.Threading.Thread.Sleep(500); BluetoothLEHardwareInterface.SubscribeCharacteristicWithDeviceAddress(_deviceAddress, FullUUID(ServiceUUID), FullUUID(GyroUUID), null, onNotification); break; case machineState.Unsubscribe: BluetoothLEHardwareInterface.UnSubscribeCharacteristic(_deviceAddress, ServiceUUID, AccelUUID, null); SetState(machineState.Disconnect, 4f); break; case machineState.Disconnect: BluetoothLEHardwareInterface.DisconnectPeripheral(_deviceAddress, (address) => { BluetoothLEHardwareInterface.DeInitialize(() => { _state = machineState.None; }); }); break; } } } }
void SetState(machineState newState, float timeout) { _state = newState; _timeout = timeout; }
private void onNotification(String address, String uuid, byte[] raw) { // we don't have a great way to set the state other than waiting until we actually got // some data back. if (_state == machineState.Subscribe) { _state = machineState.None; } if (state == State.Connected) { state = State.Receiving; } Debug.Log("UUID " + uuid); if (uuid == FullUUID(AccelUUID)) { int[] new_data = rawToIntTab(raw); // Quick LowPassFilter // + Correcting a bug on the firmware, X and Y axis are currently inverted accel [0] = (int)(new_data [1] * 0.3 + accel [0] * 0.7); accel [1] = (int)(new_data [0] * 0.3 + accel [1] * 0.7); accel [2] = (int)(new_data [2] * 0.3 + accel [2] * 0.7); } else if (uuid == FullUUID(GyroUUID)) { int[] new_data = rawToIntTab(raw); gyro = new_data; } else if (uuid == FullUUID(TouchUUID)) { #if OLD_APIDOU int new_touch; int tmp; new_touch = (int)raw [0]; // Only the feets stayed at the same place, so wipe the rest // before adding them manually tmp = new_touch & 0x11; // hands to belly if (((new_touch & 4) != 0) || ((new_touch & 8) != 0)) { tmp |= 4; } // Old ears to new ears if ((new_touch & 16) != 0) { tmp |= 8; } if ((new_touch & 32) != 0) { tmp |= 16; } touch = (uint)tmp; #else touch = (uint)raw [0]; #endif } if (uuid == FullUUID(AccelUUID) || uuid == FullUUID(GyroUUID)) { posCalc.updateData(accel, gyro); } }
void Disconnected(String uuid) { _state = machineState.Disconnect; state = State.Disconnected; Debug.Log("Disconnected from " + uuid); }