diff --git a/COPYRIGHT b/COPYRIGHT index 848f565..183461c 100644 --- a/COPYRIGHT +++ b/COPYRIGHT @@ -1,7 +1,7 @@ Copyright (c) 2018 Brain Churts All rights reserved. -This was developed by Christopher W. Olsen for the Brain Churts Project. +This was developed by Christopher W. Olsen for the Brain Churts Experiment. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: @@ -11,7 +11,7 @@ 2) Redistributions in binary form must reproduce the above copyright notice, this list of conditions, the following disclaimer and the list of authors in the documentation and/or other materials provided with the distribution. -3) Neither the name of the UbixOS Project nor the names of its contributors may be used to +3) Neither the name of the Brain Churts Experiment nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. diff --git a/Sketch/Zombot/Zombot.ino b/Sketch/Zombot/Zombot.ino new file mode 100644 index 0000000..58830b7 --- /dev/null +++ b/Sketch/Zombot/Zombot.ino @@ -0,0 +1,276 @@ +/* + Copyright (c) 2018 Brain Churts + All rights reserved. + + This was developed by Christopher W. Olsen for the Brain Churts Experiment. + + Redistribution and use in source and binary forms, with or without modification, are permitted + provided that the following conditions are met: + + 1) Redistributions of source code must retain the above copyright notice, this list of + conditions, the following disclaimer and the list of authors. + 2) Redistributions in binary form must reproduce the above copyright notice, this list of + conditions, the following disclaimer and the list of authors in the documentation and/or + other materials provided with the distribution. + 3) Neither the name of the Brain Churts Experiment nor the names of its contributors may be used to + endorse or promote products derived from this software without specific prior written + permission. + + THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS + BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "CurieIMU.h" + +//#define ACCELEROMETER_SENSITIVITY 8192.0 +#define GYROSCOPE_SENSITIVITY 16.348 + +#define M_PI 3.14159265359 +#define dt (10.0/1000.0) // 100hz = 10ms + +#define RMotor_offset 60 // The offset of the Motor +#define LMotor_offset 50 // The offset of the Motor + +// PID Settings +static float kp = 70.00f; +static float ki = 2.500f; +static float kd = 6000.00f; + +static uint32_t _prev = millis(); +static uint32_t lastTime = millis(); + +// Pin Assignments +int TN1 = 5; +int TN2 = 4; +int ENA = 9; +int TN3 = 7; +int TN4 = 6; +int ENB = 8; + +static float error = 0; // Proportion +static float errSum = 0; +static float dErr = 0; + +struct Motor { + float LOutput; + float ROutput; +} motor_t; + +static void ComplementaryFilter(short accData[3], short gyrData[3], float *pitch, float *roll) { + float pitchAcc, rollAcc; + + // Integrate the gyroscope data -> int(angularSpeed) = angle + *pitch += ((float)gyrData[0] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the X-axis + *roll -= ((float)gyrData[1] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the Y-axis + + // Compensate for drift with accelerometer data if !bullshit + // Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192 + int forceMagnitudeApprox = abs(accData[0]) + abs(accData[1]) + abs(accData[2]); + + if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768) { + // Turning around the X axis results in a vector on the Y-axis + pitchAcc = atan2f((float)accData[1], (float)accData[2]) * 180 / M_PI; + *pitch = *pitch * 0.98 + pitchAcc * 0.02; + + // Turning around the Y axis results in a vector on the X-axis + rollAcc = atan2f((float)accData[0], (float)accData[2]) * 180 / M_PI; + *roll = *roll * 0.98 + rollAcc * 0.02; + } +} + +void setup() { + Serial.begin(115200); // initialize Serial communication + //while (!Serial); // wait for the serial port to open + + // initialize device + Serial.println("Initializing IMU device..."); + + // CurieIMU.initialize(); + // CurieIMU.setFullScaleAccelRange(BMI160_ACCEL_RANGE_2G); + // CurieIMU.setFullScaleGyroRange(BMI160_GYRO_RANGE_2000); + // // CurieIMU.setGyroRate(BMI160_GYRO_RATE_100HZ); + // // CurieIMU.setFullScaleGyroRange(BMI160_GYRO_RANGE_2000); + // CurieIMU.setGyroRate(BMI160_GYRO_RATE_3200HZ); + // // CurieIMU.setGyroDLPFMode(BMI160_DLPF_MODE_NORM); + + CurieIMU.begin(); + CurieIMU.setAccelerometerRange(2); + CurieIMU.setGyroRange(2000); + CurieIMU.setGyroRate(3200); + + Serial.print("full scale accel \t"); + Serial.print(CurieIMU.getAccelerometerRange()); + Serial.print("full scale gyro \t"); + Serial.println(CurieIMU.getGyroRange()); + + // verify connection + //Serial.println("Testing device connections..."); + // if (CurieIMU.testConnection()) { + // Serial.println("CurieIMU connection successful"); + // } else { + // Serial.println("CurieIMU connection failed"); + // } + + // use the code below to calibrate accel/gyro offset values + Serial.println("Internal sensor offsets BEFORE calibration..."); + Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); + Serial.print("\t"); // -76 + Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); + Serial.print("\t"); // -235 + Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); + Serial.print("\t"); // 168 + Serial.print(CurieIMU.getGyroOffset(X_AXIS)); + Serial.print("\t"); // 0 + Serial.print(CurieIMU.getGyroOffset(Y_AXIS)); + Serial.print("\t"); // 0 + Serial.println(CurieIMU.getGyroOffset(Z_AXIS)); + + pinMode(TN1, OUTPUT); + pinMode(TN2, OUTPUT); + pinMode(TN3, OUTPUT); + pinMode(TN4, OUTPUT); + pinMode(ENA, OUTPUT); + pinMode(ENB, OUTPUT); + + Serial.println("About to calibrate. Make sure your board is stable and upright"); + //delay(10000); + + // The board must be resting in a horizontal position for + // the following calibration procedure to work correctly! + Serial.print("Starting Gyroscope calibration..."); + //CurieIMU.autoCalibrateGyroOffset(); + Serial.println(" Done"); + Serial.print("Starting Acceleration calibration..."); + //CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS,0); + //CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS,0); + //CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS,1); + Serial.println(" Done"); + + Serial.println("Internal sensor offsets AFTER calibration..."); + Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); + Serial.print("\t"); // -76 + Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); + Serial.print("\t"); // -235 + Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); + Serial.print("\t"); // 168 + Serial.print(CurieIMU.getGyroOffset(X_AXIS)); + Serial.print("\t"); // 0 + Serial.print(CurieIMU.getGyroOffset(Y_AXIS)); + Serial.print("\t"); // 0 + Serial.println(CurieIMU.getGyroOffset(Z_AXIS)); + //delay(10000); + + Serial.println("Enabling Gyroscope/Acceleration offset compensation"); + //CurieIMU.setGyroOffsetEnabled(true); + //CurieIMU.setAccelOffsetEnabled(true); + _prev = millis(); +} + + +void myPID(float filtered_angle, Motor *motor) { + static float lastErr = 0; + uint32_t timeChange = (millis() - lastTime); + static float Output; + lastTime = millis(); + + error = filtered_angle; // Proportion + errSum += error * timeChange; // Integration + dErr = (error - lastErr) / timeChange; // Differentiation + Output = kp * error + ki * errSum + kd * dErr; + lastErr = error; + + motor->LOutput = Output; + motor->ROutput = Output; + Serial.println(Output); + +} + +static void PWMControl(float LOutput, float ROutput) { + + if (LOutput > 0) { + digitalWrite(TN1, 0); + digitalWrite(TN2, 1); + } + else if (LOutput < 0) { + digitalWrite(TN1, 1); + digitalWrite(TN2, 0); + } + else { + digitalWrite(ENA, 0); + } + + if (ROutput > 0) { + digitalWrite(TN3, 1); + digitalWrite(TN4, 0); + } + else if (ROutput < 0) { + digitalWrite(TN3, 0); + digitalWrite(TN4, 1); + } + else { + digitalWrite(ENB, 0); + } + + analogWrite(ENA, min(255, (abs(LOutput) + LMotor_offset))); + analogWrite(ENB, min(255, (abs(ROutput) + RMotor_offset))); +} + +void readIMUSensor(float *Angle_Filtered) { + static float roll; + static float pitch; + + // put your main code here, to run repeatedly: + short accData[3]; + short gyrData[3]; + + CurieIMU.getMotion6(&accData[0], &accData[1], &accData[2], &gyrData[0], &gyrData[1], &gyrData[2]); + + // read accelerometer: + /* + accData[0] = CurieIMU.readAccelerometer(X_AXIS); + accData[1] = CurieIMU.readAccelerometer(Y_AXIS); + accData[2] = CurieIMU.readAccelerometer(Z_AXIS); + + gyrData[0] = CurieIMU.readGyro(X_AXIS); + gyrData[1] = CurieIMU.readGyro(Y_AXIS); + gyrData[2] = CurieIMU.readGyro(Z_AXIS); + //CurieIMU.readGyroScaled(&gyrData[0], &gyrData[1], &gyrData[2]); + */ + ComplementaryFilter(accData, gyrData, &pitch, &roll); + + *Angle_Filtered = pitch; + Serial.println(String("Angle Filter = ") + String(pitch)); + +} + + +void loop() { + static Motor motor; + static float Angle_Filtered; + + if ( millis() - _prev > dt * 1000 ) { + + _prev = millis(); + readIMUSensor(&Angle_Filtered); + + // If angle > 45 or < -45 then stop the robot + if (abs(Angle_Filtered) < 45) { + myPID(Angle_Filtered, &motor); + PWMControl(motor.LOutput, motor.ROutput); + } + else { + //TODO:// set zero when robot down. + // Output = error = errSum = dErr = 0; + Serial.println("STOP"); + analogWrite(ENA, 0); // Stop the wheels + analogWrite(ENB, 0); // Stop the wheels + } + } +} +