Processingでm2xのデータをダウンロード

	import java.io.InputStreamReader;
	import java.io.BufferedInputStream;
	import java.net.HttpURLConnection;
	import java.net.URL;
	Table dataTable;
	String filePath;
	final String dataFile="dataFile.csv";
	final String[] stream={"Temp1", "Temp2", "Temp3", "Temp4", "Humid1", "Humid2", "Humid3", "Humid4"};
	final String apiKey="xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx";
	final String deviceId="yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy";
	void setup() {
	tableSetup();
	getData();
	exit();
	}
	void draw() {
	}
	void getData() {
	String request;
	String startTime="";
	for (int i=0; i<stream.length; i++) {
	int lastIdx=dataTable.getInt(0, stream[i]);
	if (lastIdx==0) {
	request="?sort=timestamp&dir=desc&limit=1";
	}
	else {
	startTime=dataTable.getString(lastIdx, "time"+stream[i]);
	request="?sort=timestamp&dir=asc&limit=1000&start="+startTime;
	}
	try {
	String s="http://api-m2x.att.com/v2/devices/"+deviceId+"/streams/"+stream[i]+"/values.csv"+request;
	println(s);
	URL url = new URL(s);
	HttpURLConnection con = (HttpURLConnection) url.openConnection();
	con.setRequestMethod("GET");
	con.setRequestProperty("X-M2X-KEY", apiKey);
	con.connect();
	int res=con.getResponseCode();
	if (res==200) {
	InputStream inputStr = new BufferedInputStream(con.getInputStream());
	BufferedReader rd= new BufferedReader(new InputStreamReader(inputStr));
	String line=rd.readLine();
	while (line!=null) {
	String[] csvData = split(line, ',');
	String time=csvData[0];
	float data=float(csvData[1]);
	println(time+", "+data);
	if (!time.equals(startTime)) {
	int idx=dataTable.getInt(0, stream[i]);
	idx++;
	if (dataTable.getRowCount()<=idx) dataTable.addRow();
	dataTable.setString(idx, "time"+stream[i], time);
	dataTable.setFloat(idx, stream[i], data);
	dataTable.setInt(0, stream[i], idx);
	}
	line=rd.readLine();
	}
	}
	else println("Http Error "+con.getErrorStream());
	}
	catch(IOException e) {
	println("EXCEPTION "+e);
	}
	}
	saveTable(dataTable, filePath+dataFile);
	println("File saved: "+filePath+dataFile);
	}
	void tableSetup() {
	filePath=sketchPath("");
	File f=new File(filePath+dataFile);
	if (f.exists()) {
	dataTable=loadTable(filePath+dataFile, "header");
	println("File loaded: "+filePath+dataFile);
	}
	else {
	dataTable = new Table();
	for (int i=0; i<stream.length; i++) {
	dataTable.addColumn("time"+stream[i]);
	dataTable.addColumn(stream[i]);
	}
	dataTable.addRow();
	for (int i=0; i<stream.length; i++) {
	dataTable.setInt(0, stream[i], 0);
	}
	saveTable(dataTable, filePath+dataFile);
	println("File created: "+filePath+dataFile);
	}
	}

view raw m2x0561c.pde hosted with ❤ by GitHub

COTTON虫Ver2

<script src="https://gist.github.com/kirakulabo/c615397ce78a164231648f80f736849e.js"></script>

GR-ADZUKIで倒立振子

	/*GR-ADZUKI Sketch Template Version: V1.02*/
	//wheele dia ~= 50mm
	//Gear ratio ~= 114 (TAMIYA Double Gear Box)
	//Battery: Alkaline x 3
	//MPU/Driver: GR-ADZUKI
	//Gyro: MPU6050 (I2C)
	//SW1: Circle
	#include "Arduino.h"
	#include <Wire.h>
	#define LED5 13
	#define leftFwd 6
	#define leftRvs 11
	#define rightFwd 5
	#define rightRvs 10
	#define ON LOW //LOW active LED
	#define OFF HIGH
	#define SW1 2
	#define SW2 3
	#define BATT A3
	void resetPara();
	void resetVar();
	void getGyro();
	void readGyro();
	void calcTarget();
	void motor();
	void selectMode();
	void calibrate();
	bool laid();
	bool upright();
	bool standing();
	void getBaseline();
	void drive();
	void resetMotor();
	void adaptCalibAccX();
	void sendSerial();
	void drvMotorR(int pwm);
	void drvMotorL(int pwm);
	int state=-1;
	int cycleCount=0;
	unsigned long time1=0;
	unsigned long time0=0;
	float power, powerR, powerL, yawPower;
	int ipowerR, ipowerL;
	float Kang, Komg, Kdst, Kspd, Kyaw, KIang, KIdst;
	float gyroZdps, accXg;
	float varAng, varOmg, varSpd, varDst, varIang, varIdst;
	float aveDst, aveDst0;
	int accX, accY, accZ, temp, gyroX, gyroY, gyroZ;
	float gyroZoffset, gyroYoffset, accXoffset;
	float gyroZdata, gyroYdata, accXdata, accZdata;
	float aveAccX=0.0;
	float aveAccZ=0.0;
	float moveStep, moveTarget;
	float spinStep, spinTarget;
	float orientation;
	int motorRdir=0; //0:stop 1:+ -1:-
	int motorLdir=0;
	int overSpdCount;
	float battFactor=1.3; // 2016.07.14
	float bVolt;
	float aveVolt=4.5;
	bool usbPower=false;
	bool serialMonitor=false;
	const float gyroLSB=1.0/131.0; // deg/sec
	const float accLSB=1.0/16384.0; // g
	const float clk = 0.01; // in sec,
	const unsigned long interval = (unsigned long) (clk*1000.0); // in msec
	const float filterConst = 0.1;
	const float maxSpd=250.0;
	const float battConst=6.6/1023.0;
	const int driverHLoffset=27; //bigger : backword
	const float mechFactorR=0.55;
	const float mechFactorL=0.55;
	const int backlashSpd=30;
	void setup() {
	analogWriteFrequency(25000);
	pinMode(leftFwd,OUTPUT);
	pinMode(leftRvs,OUTPUT);
	pinMode(rightFwd,OUTPUT);
	pinMode(rightRvs,OUTPUT);
	pinMode(LED5, OUTPUT);
	pinMode(SW1, INPUT_PULLUP);
	pinMode(SW2, INPUT_PULLUP);
	digitalWrite(LED5, OFF);
	Serial.begin(115200);
	Wire.begin();
	Wire.beginTransmission(0x68); //Gyro sensor
	Wire.write(0x6B); // Power management register
	Wire.write(0); // wake up MPU-6050
	Wire.endTransmission(true);
	delay(500);
	checkVoltage();
	resetPara();
	resetVar();
	selectMode();
	delay(1000);
	}
	void loop(){
	monitorVoltage();
	getGyro();
	switch (state) {
	case -1:
	calcTarget();
	if (upright()) state =0;
	break;
	case 0: //baseline
	calibrate();
	state=1;
	break;
	case 1: //run
	if (standing() && overSpdCount <= 20) {
	calcTarget();
	drive();
	}
	else {
	resetMotor();
	resetVar();
	if (upright()) state=0;
	else state=9;
	}
	break;
	case 2: //wait until upright
	if (upright()) state=0;
	break;
	case 9: //fell
	if (laid()) state=2;
	break;
	}
	cycleCount += 1;
	if (cycleCount >= 100) {
	cycleCount = 0;
	adaptCalibAccX();
	if (serialMonitor) sendSerial();
	}
	do time1 = millis();
	while (time1 - time0 < interval);
	time0 = time1;
	}
	void resetPara() {
	Kang=150.0;//150
	Komg=3.0;//3.0
	KIang=1500.0;//1200
	Kyaw=15.0;//15
	Kdst=80.0;//80
	Kspd=3.5;//3.5
	KIdst=0.0;
	}
	void calcTarget() {
	spinTarget += spinStep;
	moveTarget += moveStep;
	}
	void drive() {
	varSpd += power * clk;
	varDst += Kdst * (varSpd * clk - moveTarget);
	aveDst = aveDst * 0.99 + varDst * 0.01;
	varIang += KIang * varAng * clk;
	varIdst += KIdst * varDst * clk;
	power = varIang + varIdst + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
	if (abs(power) > 300.0) overSpdCount += 1;
	else overSpdCount =0;
	if (overSpdCount > 20) return;
	yawPower = (orientation - spinTarget) * Kyaw;
	powerR = power + yawPower;
	powerL = power - yawPower;
	motor();
	}
	void motor() {
	ipowerR = (int) (constrain(powerR * mechFactorR * battFactor, -maxSpd, maxSpd));
	if (ipowerR > 0) {
	if (motorRdir == 1) drvMotorR(ipowerR);
	else drvMotorR(ipowerR + backlashSpd);
	motorRdir = 1;
	}
	else if (ipowerR < 0) {
	if (motorRdir == -1) drvMotorR(ipowerR);
	else drvMotorR(ipowerR - backlashSpd);
	motorRdir = -1;
	}
	else {
	drvMotorR(0);
	motorRdir = 0;
	}
	ipowerL = (int) (constrain(powerL * mechFactorL * battFactor, -maxSpd, maxSpd));
	if (ipowerL > 0) {
	if (motorLdir == 1) drvMotorL(ipowerL);
	else drvMotorL(ipowerL + backlashSpd);
	motorLdir = 1;
	}
	else if (ipowerL < 0) {
	if (motorLdir == -1) drvMotorL(ipowerL);
	else drvMotorL(ipowerL - backlashSpd);
	motorLdir = -1;
	}
	else {
	drvMotorL(0);
	motorLdir = 0;
	}
	}
	void getGyro() {
	readGyro();
	gyroZdps = (gyroZdata - gyroZoffset) * gyroLSB;
	varOmg = (gyroYdata - gyroYoffset) * gyroLSB; // unit:deg/sec
	accXg = (accXdata - accXoffset) * accLSB; //unit:g
	orientation += gyroZdps * clk;
	varAng += (varOmg + (accXg * 57.3 - varAng) * filterConst ) * clk;
	// varAng += varOmg * clk;
	}
	void readGyro() {
	Wire.beginTransmission(0x68);
	Wire.write(0x3B);
	Wire.endTransmission(false); //enable incremental read
	Wire.requestFrom(0x68, 14, (int)true); //used to be requestFrom(0x68, 14, true) but got warning.
	accX=Wire.read()<<8|Wire.read(); //0x3B
	accY=Wire.read()<<8|Wire.read(); //0x3D
	accZ=Wire.read()<<8|Wire.read(); //0x3F
	temp=Wire.read()<<8|Wire.read(); //0x41
	gyroX=Wire.read()<<8|Wire.read(); //0x43
	gyroY=Wire.read()<<8|Wire.read(); //0x45
	gyroZ=Wire.read()<<8|Wire.read(); //0x47
	gyroZdata = -(float) gyroX;
	gyroYdata = (float) gyroY;
	accXdata = -(float) accZ;
	aveAccX = aveAccX * 0.9 + accXdata * 0.1;
	accZdata = -(float) accX;
	aveAccZ = aveAccZ * 0.9 + accZdata * 0.1;
	}
	bool laid() {
	if (abs(aveAccX) >13000.0) return true;
	else return false;
	}
	bool upright() {
	if (abs(aveAccZ) >13000.0) return true;
	else return false;
	}
	bool standing() {
	if (abs(aveAccZ) > 10000.0 && abs(varAng) < 40.0) return true;
	else return false;
	}
	void calibrate() {
	resetVar();
	resetMotor();
	delay(2000);
	blinkLED(2);
	delay(500);
	getBaseline();
	delay(500);
	blinkLED(2);
	}
	void getBaseline() {
	gyroYoffset=gyroZoffset=0.0;
	accXoffset=0.0;
	for (int i=1; i <= 30; i++){
	readGyro();
	gyroYoffset += gyroYdata;
	gyroZoffset += gyroZdata;
	accXoffset += accXdata;
	delay(10);
	}
	gyroZoffset /= 30.0;
	gyroYoffset /= 30.0;
	accXoffset /= 30.0;
	}
	void adaptCalibAccX() {
	if (aveDst > 100.0 && aveDst > aveDst0) accXoffset -= 20.0;
	if (aveDst < -100.0 && aveDst < aveDst0) accXoffset += 20.0;
	aveDst0=aveDst;
	}
	void resetVar() {
	power=0.0;
	moveTarget=0.0;
	spinTarget=0.0;
	orientation=0.0;
	varAng=0.0;
	varOmg=0.0;
	varDst=0.0;
	varSpd=0.0;
	varIang=0.0;
	varIdst=0.0;
	aveDst=0.0;
	aveDst0=0.0;
	}
	void resetMotor() {
	drvMotorR(0);
	drvMotorL(0);
	overSpdCount=0;
	}
	void checkVoltage() {
	usbPower=false;
	bVolt = ((float) analogRead(BATT)) * battConst;
	if (serialMonitor) Serial.println(bVolt);
	if (bVolt > 4.4) blinkLED(3);
	else if (bVolt > 4.1) blinkLED(2);
	else if (bVolt > 3.8) blinkLED(1);
	else if (bVolt > 0.5) blinkLED(200);
	else usbPower=true;
	}
	void blinkLED(int n) {
	for (int i=0; i<n; i++) {
	digitalWrite(LED5,ON);
	delay(10);
	digitalWrite(LED5,OFF);
	delay(200);
	}
	}
	void monitorVoltage() {
	digitalWrite(LED5, OFF);
	bVolt = ((float) analogRead(BATT)) * battConst;
	if (bVolt<3.4) {
	digitalWrite(LED5, ON);
	if (serialMonitor) {
	Serial.print("Batt=");
	Serial.println(bVolt);
	}
	}
	aveVolt = aveVolt * 0.99 + bVolt * 0.01;
	if (usbPower) battFactor = 1.3;
	else battFactor = 5.0 / aveVolt;
	}
	void drvMotorR(int pwm) {
	if (pwm >0) {
	digitalWrite(rightRvs, HIGH);
	analogWrite(rightFwd, constrain(256-pwm, -255, 255));
	}
	else if (pwm < 0) {
	digitalWrite(rightRvs, LOW);
	analogWrite(rightFwd, constrain(-pwm+driverHLoffset, -255, 255));
	}
	else {
	digitalWrite(rightRvs, LOW);
	analogWrite(rightFwd, constrain(pwm, -255, 255));
	}
	}
	void drvMotorL(int pwm) {
	if (pwm >0) {
	digitalWrite(leftRvs, HIGH);
	analogWrite(leftFwd, constrain(256-pwm, -255, 255));
	}
	else if (pwm < 0) {
	digitalWrite(leftRvs, LOW);
	analogWrite(leftFwd, constrain(-pwm+driverHLoffset, -255, 255));
	}
	else {
	digitalWrite(leftRvs, LOW);
	analogWrite(leftFwd, constrain(pwm, -255, 255));
	}
	}
	void selectMode() {
	if (digitalRead(SW1) == LOW) { //circle
	moveStep=0.003;
	spinStep=0.2;
	blinkLED(1);
	}
	}
	void sendSerial () {
	Serial.print(millis()-time0);
	Serial.print(" state=");Serial.print(state);
	Serial.print(" accX="); Serial.print(aveAccX);
	Serial.print(" Xoffset=");Serial.print(accXoffset);
	Serial.print(" accZ="); Serial.print(aveAccZ);
	Serial.print(" ang=");Serial.print(varAng);
	// Serial.print(" Omg="); Serial.print(varOmg);
	// Serial.print(" temp="); Serial.print(temp/340.0+36.53);
	Serial.print(" Dst="); Serial.print(varDst);
	Serial.print(" aveDst=");Serial.print(aveDst);
	Serial.print(" power="); Serial.print(power);
	// Serial.print(" ori="); Serial.print(orientation);
	Serial.print(" batt="); Serial.print(aveVolt);
	Serial.print(" bFactor=");Serial.print(battFactor);
	Serial.print(" ");
	Serial.print(millis()-time0);
	Serial.println();
	}

view raw adzuki_robo_4.ino hosted with ❤ by GitHub

棒テンプ時計精度アップ

	//DRV8835 connected to D2,3,4,5,6,9
	//MBM1422 mag sensor with 3.3V power
	//Touch sens antenna connected to A0
	//LED connected to D13
	//1H = 64 escapment wheel turn
	//1 escapment turn = 15 tic
	//tact time 100ms
	//folio calc every 4 min
	//#define NOMOTOR
	#include <ST7032.h>
	#include <RTC.h>
	#include <Wire.h>
	#define DRVPWR 9
	#define DRVMODE 2
	#define DRVAIN1 3
	#define DRVAIN2 4
	#define DRVBIN1 5
	#define DRVBIN2 6
	#define NHISTORY 64
	#define BM14 0x0E
	ST7032 lcd;
	volatile long rtcTic=1;
	long rtcLastLoop=1;
	int dataX, dataY, dataZ;
	int angle, angLast; //1deg=10
	int angMax, angMin;
	int angThresh;
	int angHyst=50; //5deg
	int folioDir, folioDirLast;
	unsigned long loopTime;
	long rtcLastFolioMove;
	long tFolio, tFolioCumulativeError;
	long cFolio=-3;
	int stepNumber=0;
	int stepNumberTarget=0;
	long rtcEscWheelHistory[NHISTORY];
	long nEscapeTurn, thresh;
	long tFolioRateError;
	long nCumulativeStep=0;
	long tFolioErrorMax=0;
	long tFolioErrorMin=0;
	boolean lcdEnabled=true;
	int lcdTimerCount=900; //90 sec
	int lcdTimer;
	void setup() {
	pinMode(DRVPWR, OUTPUT);
	pinMode(DRVMODE, OUTPUT);
	pinMode(DRVAIN1, OUTPUT);
	pinMode(DRVAIN2, OUTPUT);
	pinMode(DRVBIN1, OUTPUT);
	pinMode(DRVBIN2, OUTPUT);
	pinMode(13, OUTPUT);
	digitalWrite(13, HIGH);
	digitalWrite(DRVPWR, LOW);
	digitalWrite(DRVMODE, LOW);
	Serial.begin(115200);
	setPowerManagementMode(PM_STOP_MODE);
	rtc_init();
	rtc_attach_constant_period_interrupt_handler(rtcIntHandler);
	rtc_set_constant_period_interrupt_time(RTC_CONSTANT_PERIOD_TIME_1SECOND);
	rtc_constant_period_interrupt_on();
	delay(100);
	initMag();
	lcd.begin(8,2); //(c,r)
	delay(1000);
	startup();
	Serial.println("Ready");
	}
	void loop() {
	loopTime=millis();
	getAngle();
	if (angle>angThresh+angHyst) folioDir=1;
	if (angle<angThresh-angHyst) folioDir=-1;
	if (folioDir==1 && folioDirLast==-1) folioCycle();
	folioDirLast=folioDir;
	if (rtcTic != rtcLastLoop && cFolio>0) { //executed very 1 sec
	rtcLastLoop=rtcTic;
	lcdDispStatus();
	}
	delay(50);
	touchSens();
	while(millis()-loopTime<100) delay(2);
	}
	void rtcIntHandler() {
	rtcTic++;
	}
	void startup() {
	lcd.setContrast(25);
	lcdOn();
	lcdDispText(0,0, "STEPPER");
	lcdDispText(0,1, "TRAINING");
	delay(1000);
	for (int i=0; i<20; i++) {
	stepNumberTarget+=10;
	motorStep();
	delay(200);
	}
	stepNumber=190;
	stepNumberTarget=0;
	motorStep();
	nCumulativeStep=0;
	lcdDispText(0,0, "WAIT FOR");
	lcdDispText(0,1, "FOLIO MV");
	waitFolioMotion();
	lcdDispText(0,0, "CALC ANG");
	lcdDispText(0,1, "THRESHLD");
	getThresh();
	lcdDispText(0,0, "OK 000");
	lcdDispText(0,1, "0000 000");
	}
	void folioCycle() {
	digitalWrite(13, LOW);
	delay(1);
	digitalWrite(13, HIGH);
	rtcLastFolioMove=rtcTic;
	cFolio++;
	if (cFolio==0) {
	rtcTic=0;
	cFolio=0;
	stepNumber=stepNumberTarget=0;
	tFolioErrorMax=tFolioErrorMin=0;
	nCumulativeStep=0;
	}
	else if (cFolio<=0 || rtcTic<=0) return;
	tFolio=cFolio*15/4; //escapement 1tic=3.75sec
	tFolioCumulativeError=tFolio-rtcTic;
	lcdDispFolioTic(4,1);
	lcdDispNum34(4,0, tFolioCumulativeError);
	if (tFolioCumulativeError>tFolioErrorMax) tFolioErrorMax=tFolioCumulativeError;
	if (tFolioCumulativeError<tFolioErrorMin) tFolioErrorMin=tFolioCumulativeError;
	if (cFolio%60==0) { //every 60 turn =~4min
	nEscapeTurn=cFolio/15;
	rtcEscWheelHistory[nEscapeTurn%NHISTORY]=rtcTic;
	if (nEscapeTurn>=4) tFolioRateError=225-(rtcTic-rtcEscWheelHistory[(nEscapeTurn+NHISTORY-4)%NHISTORY]); //gain against 4 turns ago
	else tFolioRateError=0;
	lcdDispNum3(5,1, tFolioRateError);
	if (tFolioRateError*tFolioCumulativeError >=0) stepNumberTarget += (int) tFolioRateError/3 + (int) tFolioCumulativeError/4;
	if (stepNumberTarget != stepNumber) {
	motorStep();
	lcdDispNum4(0,0, nCumulativeStep);
	}
	}
	}
	void touchSens() {
	int adata1=analogRead(A0); //touch sensor
	delay(1);
	int adata2=analogRead(A0);
	if (abs(adata1-adata2) > 18 || adata1+adata2 < 800) { //touch detect threshold
	if (!lcdEnabled) lcdOn();
	lcdTimer=lcdTimerCount;
	}
	else {
	if (lcdEnabled) lcdTimer--;
	if (lcdTimer<=0) lcdOff();
	}
	}
	void lcdOn() {
	lcdTimer=lcdTimerCount;
	lcd.display();
	lcdEnabled=true;
	lcdDispNum4(0,0, nCumulativeStep);
	lcdDispNum4(0,1, tFolioErrorMax);
	lcdDispNum34(4,0, tFolioCumulativeError);
	lcdDispNum3(5,1, tFolioRateError);
	}
	void lcdOff() {
	lcd.noDisplay();
	lcdEnabled=false;
	}
	void lcdDispText(int col, int row, String s) {
	if (!lcdEnabled) return;
	lcd.setCursor(col,row);
	lcd.print(s);
	}
	void lcdDispStatus() {
	if (rtcTic-rtcLastFolioMove > 6) { //disp LCD even if folio not moving
	tFolioCumulativeError=tFolio-rtcTic;
	lcdDispNum34(4,0, tFolioCumulativeError);
	}
	if (rtcTic%8==0) {
	lcdDispNum4(0,0, nCumulativeStep);
	lcdDispNum4(0,1, tFolioErrorMax);
	}
	else if (rtcTic%8==4) {
	if (rtcTic<43200) {
	lcdDispNum3(0,0, rtcTic/60);
	lcdDispText(3,0, "M");
	}
	else if (rtcTic<3600000) {
	lcdDispNum3(0,0, rtcTic/3600);
	lcdDispText(3,0, "H");
	}
	else {
	lcdDispNum3(0,0, rtcTic/86400);
	lcdDispText(3,0, "D");
	}
	lcdDispNum4(0,1, tFolioErrorMin);
	}
	}
	void lcdDispNum4(int col, int row, long n) {
	if (!lcdEnabled) return;
	char pNum[6];
	if (n>999999) sprintf(pNum, "%s", ">1+6");
	else if (n>99999) sprintf(pNum, "%02dE4", n/10000);
	else if (n>9999) sprintf(pNum, "%02dE3", n/1000);
	else if (n>=0) sprintf(pNum, "%04d", n);
	else if (n>=-999) sprintf(pNum, "%04d", n);
	else if (n>=-9999) sprintf(pNum, "%02d-2", -n/100);
	else if (n>=-99999) sprintf(pNum, "%02d-3", -n/1000);
	else if (n>=-999999) sprintf(pNum, "%02d-4", -n/10000);
	else sprintf(pNum, "%s", "<1-6");
	lcd.setCursor(col,row);
	lcd.print(pNum);
	}
	void lcdDispNum3(int col, int row, long n) {
	if (!lcdEnabled) return;
	char pNum[6];
	if (n>999) sprintf(pNum, "%s", ">k+");
	else if (n>=-99) sprintf(pNum, "%03d", n);
	else if (n>=-999) sprintf(pNum, "%02d-", -n/10);
	else sprintf(pNum, "%s", "<k-");
	lcd.setCursor(col,row);
	lcd.print(pNum);
	}
	void lcdDispNum34(int col, int row, long n) {
	if (!lcdEnabled) return;
	char pNum[6];
	if (n>999) sprintf(pNum, "%s", " >k+");
	else if (n>=-99) sprintf(pNum, " %03d", n);
	else if (n>=-999) sprintf(pNum, "%04d", n);
	else sprintf(pNum, "%s", " <k-");
	lcd.setCursor(col,row);
	lcd.print(pNum);
	}
	void lcdDispFolioTic(int col, int row) {
	if (!lcdEnabled) return;
	lcd.setCursor(col, row);
	char n=cFolio%15+0xb1; //"ア"=0,... "ソ"=14
	lcd.print(n);
	}
	void motorStep() {
	digitalWrite(DRVPWR, HIGH);
	delay(5);
	while (stepNumberTarget-stepNumber != 0) {
	if (stepNumberTarget>stepNumber) stepNumber++;
	else stepNumber--;
	nCumulativeStep++;
	#ifdef NOMOTOR
	Serial.print("Motor setp=");Serial.println(stepNumber);
	#else
	motorDrive();
	#endif
	delay(10); //motor freq=100Hz
	}
	motorOut(LOW, LOW, LOW, LOW);
	digitalWrite(DRVPWR, LOW);
	}
	void motorDrive() {
	int phase=stepNumber%4;
	if (phase<0) phase=4+phase;
	if (phase==0) motorOut(HIGH, LOW, HIGH, LOW);
	if (phase==1) motorOut(LOW, HIGH, HIGH, LOW);
	if (phase==2) motorOut(LOW, HIGH, LOW, HIGH);
	if (phase==3) motorOut(HIGH, LOW, LOW, HIGH);
	}
	void motorOut(byte out1, byte out2, byte out3, byte out4) {
	digitalWrite(DRVAIN1, out1);
	digitalWrite(DRVAIN2, out2);
	digitalWrite(DRVBIN1, out3);
	digitalWrite(DRVBIN2, out4);
	}
	void getMag() {
	magWrite(0x1D, 0x40); //Start ADC
	delay(1);
	dataX=magRead(0x10);
	dataY=magRead(0x12);
	dataY+=160;
	dataZ=magRead(0x14);
	dataZ+=50;
	}
	void initMag() {
	// AVE_A defaul=0 -> 4times averate
	magWrite(0x1B, 0x82); //CNTL1
	delay(2);
	magWrite(0x5C, 0); //CNTL4 LSB
	magWrite(0x5D, 0); //CNTL4 MSB
	magWrite(0x1C, 0x08); //CNTL2 DRDY enable with low active
	getMag();
	}
	void waitFolioMotion() {
	angMax=-3600;
	angMin=3600;
	do {
	getAngle();
	if (angle > angMax) angMax=angle;
	if (angle < angMin) angMin=angle;
	delay(100);
	} while (angMax-angMin<300);
	}
	void getThresh() {
	unsigned long msec=millis();
	angMax=-3600;
	angMin=3600;
	do {
	getAngle();
	if (angle > angMax) angMax=angle;
	if (angle < angMin) angMin=angle;
	delay(100);
	} while (millis()-msec<15000); //15second
	angThresh=(angMax+angMin)/2;
	}
	void getAngle() {
	getMag();
	angle= (int) (atan2((double)dataZ, (double)dataY)*572.9579); //1800.0/3.141592=572.9579
	int diff=angle - angLast;
	if (diff > 1800) angle-=3600;
	if (diff < -1800) angle+=3600;
	angLast=angle;
	/*
	Serial.print(dataZ);
	Serial.print(" ");
	Serial.print(dataY);
	Serial.print(" ");
	Serial.print(angle);
	*/
	}
	void magWrite (unsigned char adr, unsigned char data) {
	Wire.beginTransmission(BM14);
	Wire.write(adr);
	Wire.write(data);
	Wire.endTransmission(true);
	}
	int magRead(unsigned char adr) {
	Wire.beginTransmission(BM14);
	Wire.write(adr);
	Wire.endTransmission(false);
	Wire.requestFrom(BM14, 2, (int)true);
	return Wire.read()|Wire.read()<<8;
	}

view raw FolioClock27Mag.ino hosted with ❤ by GitHub