We Keep Coding

Unity 3D Object Rotation with MPU6050

Basically, I am calibrating and applying offsets within unity. But the range of rotation isn't accurate and I'm wondering if its because of the wrong sensitivity application. I wanted help figuring out a way to stabilize the rotation as well as increase the range of rotation as it goes only form 0-90 and it shakes a lot.
Unity Code:
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.IO.Ports;
public class MPU6050_3 : MonoBehaviour
{
public GameObject target;
SerialPort stream = new SerialPort("COM3", 115200);
string[] accgyValues = new string[10];
int buffersize = 1000; //Amount of readings used to average, make it higher to get more precision but sketch will be slower (default:1000)
int acel_deadzone = 2; //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge (default:8)
int giro_deadzone = 1; //Giro error allowed, make it lower to get more precision, but sketch may not converge (default:1)
float ax, ay, az, gx, gy, gz, gx_f, gy_f, gz_f;
float mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz, state = 0;
float ax_offset = 0, ay_offset = 0, az_offset = 0, gx_offset = 0, gy_offset = 0, gz_offset = 0;
bool cal;
public float factor = 1;
public bool enableRotation;
public bool enableTranslation;
//float acc_normalizer_factor = 0.00025f;
float gyro_normalizer_factor = 1.0f/65.5f;
void Awake()
{
stream.Open();
//stream.ReadTimeout=50;
}
void Update()
{
if (state == 0)
{
accel();
Debug.Log("\nReading sensors for first time...");
state++;
Debug.Log("State is:"+state);
}
else if (state == 1)
{
Debug.Log("\nCalculating offsets...");
calibration();
state++;
Debug.Log("State is:" + state);
}
else if (state == 2)
{
accel();
Debug.Log("SensorW/Offset:" + mean_ax + ":" + mean_ay + ":" + mean_az + ":" + mean_gx + ":" + mean_gy + ":" + mean_gz);
Debug.Log("Offset:" + ax_offset + ":" + ay_offset + ":" + az_offset + ":" + gx_offset + ":" + gy_offset + ":" + gz_offset);
state++;
Debug.Log("State is:" + state);
}
else if (state == 3)
{
getaccgy();
//if (enableTranslation) target.transform.position = new Vector3(ax+ax_offset, az-az_offset, ay-ay_offset);
gx_f = (gx + gx_offset) * gyro_normalizer_factor;
gy_f = (gy + gy_offset) * gyro_normalizer_factor;
gz_f = -(gz + gz_offset) * gyro_normalizer_factor ;
if (enableRotation) target.transform.rotation = Quaternion.Euler(gx_f * factor, gz_f * factor, gy_f * factor);
Debug.Log("ValueRot "+ gx_f +" " + gy_f + " " + gz_f);
Debug.Log("State is:" + state);
}
}
public void getaccgy()
{
stream.Write("r");
Debug.Log("In accel");
string value;
accgyValues = new string[10];
value = stream.ReadLine();
accgyValues = value.Split(",");
ax = int.Parse(accgyValues[0]); //* acc_normalizer_factor;
ay = int.Parse(accgyValues[1]); //* acc_normalizer_factor;
az = int.Parse(accgyValues[2]); //* acc_normalizer_factor;
gx = int.Parse(accgyValues[3]); //* gyro_normalizer_factor;
gy = int.Parse(accgyValues[4]); //* gyro_normalizer_factor;
gz = int.Parse(accgyValues[5]); //* gyro_normalizer_factor;
Debug.Log("GetACCGY");
Debug.Log("SensorW/Offset:" + ax + ":" + ay + ":" + az + ":" + gx + ":" + gy + ":" + gz);
}
void accel()
{
float i = 0f, buff_ax = 0f, buff_ay = 0f, buff_az = 0f, buff_gx = 0f, buff_gy = 0f, buff_gz = 0f;
while (i < 1101)
{
getaccgy();
if (cal)
{
ax = ax + ax_offset;
ay = ay + ay_offset;
az = az + az_offset;
gx = gx + gx_offset;
gy = gy + gy_offset;
gz = gz + gz_offset;
}
if (i > 99 && i < (buffersize + 100))
{ //First 100 measures are discarded
buff_ax += ax;
buff_ay += ay;
buff_az += az;
buff_gx += gx;
buff_gy += gy;
buff_gz += gz;
}
if (i == (buffersize + 100))
{
mean_ax = buff_ax / buffersize;
mean_ay = buff_ay / buffersize;
mean_az = buff_az / buffersize;
mean_gx = buff_gx / buffersize;
mean_gy = buff_gy / buffersize;
mean_gz = buff_gz / buffersize;
Debug.Log("Mean:" + mean_ax + ":" + mean_ay + ":" + mean_az + ":" + mean_gx + ":" + mean_gy + ":" + mean_gz);
}
i++;
Debug.Log("Mean Sensor");
Debug.Log("Buffer Size is" + buffersize + "i mean" + i);
Debug.Log("SensorW/Offset:" + ax + ":" + ay + ":" + az + ":" + gx + ":" + gz);
}
}
void calibration()
{
cal = true;
Debug.Log("Callibration Entered");
ax_offset = -mean_ax / 8;
ay_offset = -mean_ay / 8;
az_offset = (16384 - mean_az) / 8;
gx_offset = -mean_gx / 4;
gy_offset = -mean_gy / 4;
gz_offset = -mean_gz / 4;
int ready = 0;
while (ready < 7)
{
//accelgyro.setXAccelOffset(ax_offset);
//accelgyro.setYAccelOffset(ay_offset);
//accelgyro.setZAccelOffset(az_offset);
//accelgyro.setXGyroOffset(gx_offset);
//accelgyro.setYGyroOffset(gy_offset);
//accelgyro.setZGyroOffset(gz_offset);
accel();
Debug.Log("..." + ready);
if (Math.Abs(mean_ax) <= acel_deadzone) ready++;
else ax_offset = ax_offset - mean_ax / acel_deadzone;
if (Math.Abs(mean_ay) <= acel_deadzone) ready++;
else ay_offset = ay_offset - mean_ay / acel_deadzone;
if (Math.Abs(16384 - mean_az) <= acel_deadzone) ready++;
else az_offset = az_offset + (16384 - mean_az) / acel_deadzone;
if (Math.Abs(mean_gx) <= giro_deadzone) ready++;
else gx_offset = gx_offset - mean_gx / (giro_deadzone + 1);
if (Math.Abs(mean_gy) <= giro_deadzone) ready++;
else gy_offset = gy_offset - mean_gy / (giro_deadzone + 1);
if (Math.Abs(mean_gz) <= giro_deadzone) ready++;
else gz_offset = gz_offset - mean_gz / (giro_deadzone + 1);
if (ready == 6) break;
}
}
}
Arduino Code:
Just a simple code to get the raw values:
// I2Cdev and MPU6050 must be installed as libraries
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
/////////////////////////////////// CONFIGURATION /////////////////////////////
//Change this 3 variables if you want to fine tune the skecth to your needs.
int buffersize=1000; //Amount of readings used to average, make it higher to get more precision but sketch will be slower (default:1000)
int acel_deadzone=8; //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge (default:8)
int giro_deadzone=1; //Giro error allowed, make it lower to get more precision, but sketch may not converge (default:1)
// default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
//MPU6050 accelgyro;
MPU6050 accelgyro(0x68); // <-- use for AD0 high
int16_t ax, ay, az,gx, gy, gz;
int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;
int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;
/////////////////////////////////// SETUP ////////////////////////////////////
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
// COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.
// initialize serial communication
Serial.begin(115200);
// initialize device
accelgyro.initialize();
//wait for ready
//while (Serial.available() && Serial.read()); // empty buffer
//while (!Serial.available()){
//Serial.println(F("Send any character to start sketch.\n"));
//delay(1500);
//}
//while (Serial.available() && Serial.read()); // empty buffer again
}
// start message
//Serial.println("\nMPU6050 Calibration Sketch");
//delay(2000);
//Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
//delay(3000);
// verify connection
//Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
//delay(1000);
// reset offsets
//accelgyro.setXAccelOffset(0);
//accelgyro.setYAccelOffset(0);
//accelgyro.setZAccelOffset(0);
//accelgyro.setXGyroOffset(0);
//accelgyro.setYGyroOffset(0);
//accelgyro.setZGyroOffset(0);
void loop() {
if(Serial.available() > 0)
{
char ltr = Serial.read();
if(ltr == 'r')
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Serial.print(ax);
Serial.print(",");
Serial.print(ay);
Serial.print(",");
Serial.print(az);
Serial.print(",");
Serial.print(gx);
Serial.print(",");
Serial.print(gy);
Serial.print(",");
Serial.println(gz);
Serial.flush();
}
}
}

C# Wait until data received and go to next iteration

I'm using Serial Port to get data from device in for loop iteration.
The problem is in loop iteration i need to get data from serial port, validate it and going to the next iteration.
How i can achieved this?
Here are my code :
private void processData()
{
// Loop Procedure
int x = Int32.Parse(master["Cycle"].ToString());
int y = Int32.Parse(master["MinWeight"].ToString());
// Loop for each line
for (int i = this.CLine; i < 2; i++)
{
this.CLine = i;
if (i == 0)
label15.Text = master["LLINE"].ToString();
else
label15.Text = master["RLINE"].ToString();
IDictionary<string, string> dic = (Dictionary<String, String>)master[i.ToString()];
label18.Text = this.CProcess = dic["PROCESSID"];
int z = Int32.Parse(dic["PRODLANE"].ToString());
// Loop for each sampling session (Cycle)
for (int j = this.CCycle; j <= x; j++)
{
this.CCycle = j;
// Loop for production lane
for (int k = this.CLane; k <= z; k++)
{
this.CLane = k;
label16.Text = k.ToString();
// In this section i want to send command over serial port
// get value from my device
// validate it if current weight bellow standard weight
// do it again (get data from device)
// else we can go to next iteration
while (this.CWeight < y)
{
XApi.l("xxx2 " + this.CWeight + " vs " + y + " " + k.ToString() + " " + this.isDataReady);
SendData("Q");
}
// Commit Transaction
// XDb.CommitTrans(this.CCycle.ToString(), dic["LINEID"].ToString(), this.CLane.ToString(), weight.ToString(), this.isTrialStage == true ? "1" : "0");
}
}
}
}
I've tried this
while (this.CWeight < y)
{
XApi.l("xxx2 " + this.CWeight + " vs " + y + " " + k.ToString() + " " + this.isDataReady);
SendData("Q");
}
but it seems blocked UI thread and make my application solaggy.
Anyone can give me some idea? Thanks in advance.
private void port_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
if (e.EventType == System.IO.Ports.SerialData.Eof)
return;
// If the com port has been closed, do nothing
if (!comport.IsOpen)
return;
// Update flag data received
this.isDataReady = true;
// Determain which mode (string or binary) the user is in
if (CurrentDataMode == DataMode.Text)
{
// Read all the data waiting in the buffer
string data = comport.ReadExisting();
// Update result
result += data;
if (result.Length > 16)
{
SetText(result.ToString());
}
// Display the text to the user in the terminal
Log(LogMsgType.Incoming, data);
}
else
{
// Obtain the number of bytes waiting in the port's buffer
int bytes = comport.BytesToRead;
// Create a byte array buffer to hold the incoming data
byte[] buffer = new byte[bytes];
// Read the data from the port and store it in our buffer
comport.Read(buffer, 0, bytes);
// Show the user the incoming data in hex format
Log(LogMsgType.Incoming, ByteArrayToHexString(buffer));
}
}
private void SendData(String msg)
{
this.isDataReady = false;
result = "";
if (CurrentDataMode == DataMode.Text)
{
// Send the user's text straight out the port
comport.Write(msg + "\r\n");
// Show in the terminal window the user's text
Log(LogMsgType.Outgoing, msg + "\n");
}
else
{
try
{
// Convert the user's string of hex digits (ex: B4 CA E2) to a byte array
byte[] data = HexStringToByteArray(txtSendData.Text);
// Send the binary data out the port
comport.Write(data, 0, data.Length);
// Show the hex digits on in the terminal window
Log(LogMsgType.Outgoing, ByteArrayToHexString(data) + "\n");
}
catch (FormatException)
{
// Inform the user if the hex string was not properly formatted
Log(LogMsgType.Error, "Not properly formatted hex string: " + txtSendData.Text + "\n");
}
}
}

Anyone can give me some idea?
You can use async/await in your code not to block your UI by writing an extension method like below. Usage would be:
async void SomeMethod()
{
SerialPort serialPort = .......
while (true)
{
serialPort.Write(.....);
var retval = await serialPort.ReadAsync();
}
}
The keyword here is using TaskCompletionSource class with your events...
static public class SerialPortExtensions
{
public static Task<byte[]> ReadAsync(this SerialPort serialPort)
{
var tcs = new TaskCompletionSource<byte[]>();
SerialDataReceivedEventHandler dataReceived = null;
dataReceived = (s, e) =>
{
serialPort.DataReceived -= dataReceived;
var buf = new byte[serialPort.BytesToRead];
serialPort.Read(buf, 0, buf.Length);
tcs.TrySetResult(buf);
};
serialPort.DataReceived += dataReceived;
return tcs.Task;
}
}

how to use text based console progress bar in multithreading C#

Hi I have a program that downloads + extracts the files in parallel (in threads).
it is a console app,and I want to show the Progress Bar for each operation in each thread.
for eg:
File 1 [==========35% ] 35mb of 100mb downloaded
File 2 [====20% ] 20mb of 100mb downloaded
File1 Downloaded,
File 1 [=============50% ] 50% extracted.
and so on.
note: I am able to show the console outputs as Code below, but would like to use this Progress Bar in my Console APP.
How can I use solution proposed in https://gist.github.com/DanielSWolf/0ab6a96899cc5377bf54 in this case ?
public static void DownloadAndGetFiles()
{
try
{
Parallel.ForEach(FileIds, currentId =>
{
int currentId = FileIds.Id
clientFileDownload(currentId);
});
}
catch (Exception e)
{
}
}
private static void clientFileDownload(int currentId)
{
WebClient client = new WebClient();
client.DownloadProgressChanged += new DownloadProgressChangedEventHandler(client_DownloadProgressChanged);
client.DownloadFileCompleted += new AsyncCompletedEventHandler(client_DownloadFileCompleted);
string downloadedFile = #"d:\tmp\";
client.DownloadFileAsync(new Uri(currentId.URL), downloadedFile); //some URL
while (client.IsBusy) { }
string temp = ExtractAndRename(currentId);
}
private static void client_DownloadProgressChanged(object sender, DownloadProgressChangedEventArgs e)
{
//Prints: "Downloaded 3mb of 61.46mb (4%)"
Console.WriteLine("Downloaded "
+ ((e.BytesReceived / 1024f) / 1024f).ToString("#0.##") + "mb"
+ " of "
+ ((e.TotalBytesToReceive / 1024f) / 1024f).ToString("#0.##") + "mb"
+ " (" + e.ProgressPercentage + "%)");
}
private static string ExtractAndRename(int currentId)
{
//using SevenZipExtractor lib http://stackoverflow.com/questions/20898794/how-to-extract-files-in-my-archive-one-by-one-using-sevenzipsharp
SevenZipExtractor extractor = new SevenZipExtractor(#"d:\tmp\" + id.Name);
extractor.Extracting += extractor_Extracting;
extractor.ExtractArchive(#"d:\tmp\" + extractName[0]);
return (#"d:\tmp\" + extractName[0]);
}
public static void extractor_Extracting(object sender, SevenZip.ProgressEventArgs p)
{
Console.ForegroundColor = ConsoleColor.Yellow;
Console.Write("\b\b{0}% Extracted", p.PercentDone);
Console.ResetColor();
}

Provide every thread with a variable y which contains the line number it is allowed to write to.
Before a thread wants to update the screen, create a lock. The console can be used by only one thread at a time. Otherwise results of several threads will mix up.
Move the cursor to line specified by y and update that line.
Release the lock.
An example:
static private readonly object _sync = new object();
private static void UpdateProgress(int y, string item, int progress, int total)
{
int percentage = (int)100.0 * progress / total;
lock(_sync)
{
Console.CursorLeft = 0;
Console.CursorTop = y;
Console.Write(item + " [" + new string('=', percentage / 2) + "] " + percentage + "%");
}
}
You can call this method from your method clientFileDownload, which has to be modified a bit:
private static void clientFileDownload(int currentId, int y)
and should be called when creating the threads like this:
int y = 0;
Parallel.ForEach(FileIds, currentId =>
{
int currentId = FileIds.Id
clientFileDownload(currentId, y);
Interlocked.Increment(ref y);
});

Writing items from listbox to text file in C# with try-catch statment

My code it quite long but this is the part that I am stuck on. I have my try statement at the beginning before my for loops. But now I want to take my information that is in the ListBox and send it to a text file which I already have in my debug folder. I would like to just take my outputs from the list box and write them to the population.txt file.
//variable user input, store in userInput
try
{
if (double.TryParse(startTextbox.Text, out start))
{
if (double.TryParse(averageTextbox.Text, out average))
{
if (double.TryParse(daysTextbox.Text, out days))
{
//process
int count = 1;
while (count <= days)
{
//calculation
double output;
output = start * Math.Pow((1 + average / 100), count - 1);
//display the results in the listbox
populationListBox.Items.Add("The approximate population for " +
count + " day(s) is " + output.ToString("n2"));
//count the days
count = count + 1;
}
//used to text statement
//populationListBox.Items.Add("End of while loop");
count = 1;
do
{
//calculation
double output;
output = start * Math.Pow((1 + average / 100), count - 1);
//display the results in the listbox
populationListBox.Items.Add("The approximate population for " +
count + " day(s) is " + output.ToString("n2"));
//count the days
count = count + 1;
} while (count <= days);
//used to text statement
//populationListBox.Items.Add("End of do-while loop");
//int count;
for (count = 1; count <= days; )
{
//calculation
double output;
output = start * Math.Pow((1 + average / 100), count - 1);
//display the results in the listbox
populationListBox.Items.Add("The approximate population for " +
count + " day(s) is " + output.ToString("n2"));
//count the days
count = count + 1;
}
//used to text statement
//populationListBox.Items.Add("End of for loop");
}
else
{
//error message for input
MessageBox.Show("Invalid input for number of days to multiply.");
}
}
else
{
//error message for input
MessageBox.Show("Invalid input for average daily increase.");
}
}
else
{
//error message for input
MessageBox.Show("Invalid input for starting days");
}
StreamWriter outputFile;
outputFile = File.CreateText("population.txt");
outputFile.WriteLine("Approximate population: ");
outputFile.WriteLine(populationListBox.Items);
outputFile.ToString();
outputFile.Close();
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
}
}

You can use library like FileHelper to perform your job. It is open source and free. If you want to use just FileIO from .NET framework you can do that also
using (StreamWriter sr = File.CreateText("population.txt"))
{
foreach (string s in listBox.Items)
{
sr.WriteLine(s);
}
}

create one time reference for various sample rates per file

My program reads in multiple files that contain time/value pairs sampled at different rates. I'm trying to use the file with the highest sample rate as the time scale for all sampled rates and output one master file with the unique time values from the highest sample rate file.
Each file contains time/values pairs like:
1,58
1.5,90
2,154
2.5,34
Here is my code so far:
public void ReadAndWrite(string[] fileNames)
{
var stopwatch = Stopwatch.StartNew();
List<StreamReader> readers = fileNames.Select(f => new StreamReader(f)).ToList();
try
{
using (StreamWriter writer = new StreamWriter(tbxOutputFile.Text))
{
string line = null;
// For each measurement in max measurements
for (int measNum = 0; measNum < numOfRows; measNum++)
{
// For each file's reader
for (int i = 0; i < readers.Count; i++)
{
// If line contains something, then add it to line
if ((line = readers[i].ReadLine()) != null)
{
// Process line and then write it to file
line = ProcessLine(line);
writer.Write(line);
}
else
{
writer.Write("");
}
// If it's not the last column, add delimiter
if (i < readers.Count - 1)
writer.Write(",");
}
writer.WriteLine();
// Update labels
int val = ((measNum + 1) * 100) / numOfRows;
string newText = Convert.ToString("Progress: " + val + "% " + " " + "Measurement #: " + (measNum + 1)
+ " out of " + numOfRows); // running on worker thread
this.Invoke((MethodInvoker)delegate
{
// runs on UI thread
lblStatus.Text = newText;
progressBar1.Value = val;
});
}
}
}
catch (Exception)
{
throw;
}
finally
{
foreach (var reader in readers)
{
reader.Close();
}
}
MessageBox.Show("File successfully created! " + '\n' + "Elapsed time: " +
(stopwatch.ElapsedMilliseconds/1000) + " seconds", "Processing Complete");
}
I came up with the pseudo code below (currentTime is the time from each file and uniqueTime is from an array that reads in each time from the highest sampled file):
// if time value from individual file is same as uniqueTime
if currentTime == uniqueTime
{
valueToWrite = curr_value // write the current value
}
else // currentTime is not same as uniqueTime
{
valueToWrite = prev_value // write the previous value
}
timeToWrite = uniqueTime // always write the uniqueTime
What is the best way to execute this pseudo code to make a unique time reference for all the various sample rates? Sorry if my question is confusing, I can elaborate more if need be.

To be clear about this, you do not want the values at the specific time they occurred, but you want to display one value for each source at each timepoint that the highest sampled source has?
That should be pretty straightforward. In pseudocode:
foreach (ValuePair val in highSampleRateValues) {
var aggregatedTimePointData;
aggregatedTimePointData.Add(val.Time, val.Value);
foreach (ValuePair val2 in lowSampleRateValues) {
var value = DetermineLatestEntryBackwardFrom(val.Time);
aggregatedTimePointData.Add(value);
}
}
This way, the sample rate of the higher density sampled signal serves as a clock, but you will have inaccuracies since the values from the other sources are only close, but not exactly on the timepoint of their recording. If you want those inaccuracies reduced, choose a higher sample rate and do the same thing. You can get as close to the actual timepoints as you want.