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);
}
