├── I2C.ino ├── Kalman.cpp ├── Kalman.h ├── LICENSE ├── Onewheel_July.ino ├── README.md ├── UARTcontrol └── Wire.h /I2C.ino: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB 19 | const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication 20 | 21 | uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) { 22 | return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success 23 | } 24 | 25 | uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) { 26 | Wire.beginTransmission(IMUAddress); 27 | Wire.write(registerAddress); 28 | Wire.write(data, length); 29 | uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success 30 | if (rcode) { 31 | Serial.print(F("i2cWrite failed: ")); 32 | Serial.println(rcode); 33 | } 34 | return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission 35 | } 36 | 37 | uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) { 38 | uint32_t timeOutTimer; 39 | Wire.beginTransmission(IMUAddress); 40 | Wire.write(registerAddress); 41 | uint8_t rcode = Wire.endTransmission(false); // Don't release the bus 42 | if (rcode) { 43 | Serial.print(F("i2cRead failed: ")); 44 | Serial.println(rcode); 45 | return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission 46 | } 47 | Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading 48 | for (uint8_t i = 0; i < nbytes; i++) { 49 | if (Wire.available()) 50 | data[i] = Wire.read(); 51 | else { 52 | timeOutTimer = micros(); 53 | while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available()); 54 | if (Wire.available()) 55 | data[i] = Wire.read(); 56 | else { 57 | Serial.println(F("i2cRead timeout")); 58 | return 5; // This error value is not already taken by endTransmission 59 | } 60 | } 61 | } 62 | return 0; // Success 63 | } 64 | -------------------------------------------------------------------------------- /Kalman.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | #include "Kalman.h" 19 | 20 | Kalman::Kalman() { 21 | /* We will set the variables like so, these can also be tuned by the user */ 22 | Q_angle = 0.001f; 23 | Q_bias = 0.003f; 24 | R_measure = 0.03f; 25 | 26 | angle = 0.0f; // Reset the angle 27 | bias = 0.0f; // Reset bias 28 | 29 | P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical 30 | P[0][1] = 0.0f; 31 | P[1][0] = 0.0f; 32 | P[1][1] = 0.0f; 33 | }; 34 | 35 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 36 | float Kalman::getAngle(float newAngle, float newRate, float dt) { 37 | // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 38 | // Modified by Kristian Lauszus 39 | // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it 40 | 41 | // Discrete Kalman filter time update equations - Time Update ("Predict") 42 | // Update xhat - Project the state ahead 43 | /* Step 1 */ 44 | rate = newRate - bias; 45 | angle += dt * rate; 46 | 47 | // Update estimation error covariance - Project the error covariance ahead 48 | /* Step 2 */ 49 | P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); 50 | P[0][1] -= dt * P[1][1]; 51 | P[1][0] -= dt * P[1][1]; 52 | P[1][1] += Q_bias * dt; 53 | 54 | // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") 55 | // Calculate Kalman gain - Compute the Kalman gain 56 | /* Step 4 */ 57 | float S = P[0][0] + R_measure; // Estimate error 58 | /* Step 5 */ 59 | float K[2]; // Kalman gain - This is a 2x1 vector 60 | K[0] = P[0][0] / S; 61 | K[1] = P[1][0] / S; 62 | 63 | // Calculate angle and bias - Update estimate with measurement zk (newAngle) 64 | /* Step 3 */ 65 | float y = newAngle - angle; // Angle difference 66 | /* Step 6 */ 67 | angle += K[0] * y; 68 | bias += K[1] * y; 69 | 70 | // Calculate estimation error covariance - Update the error covariance 71 | /* Step 7 */ 72 | float P00_temp = P[0][0]; 73 | float P01_temp = P[0][1]; 74 | 75 | P[0][0] -= K[0] * P00_temp; 76 | P[0][1] -= K[0] * P01_temp; 77 | P[1][0] -= K[1] * P00_temp; 78 | P[1][1] -= K[1] * P01_temp; 79 | 80 | return angle; 81 | }; 82 | 83 | void Kalman::setAngle(float angle) { this->angle = angle; }; // Used to set angle, this should be set as the starting angle 84 | float Kalman::getRate() { return this->rate; }; // Return the unbiased rate 85 | 86 | /* These are used to tune the Kalman filter */ 87 | void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; }; 88 | void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; }; 89 | void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; }; 90 | 91 | float Kalman::getQangle() { return this->Q_angle; }; 92 | float Kalman::getQbias() { return this->Q_bias; }; 93 | float Kalman::getRmeasure() { return this->R_measure; }; 94 | -------------------------------------------------------------------------------- /Kalman.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | #ifndef _Kalman_h_ 19 | #define _Kalman_h_ 20 | 21 | class Kalman { 22 | public: 23 | Kalman(); 24 | 25 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 26 | float getAngle(float newAngle, float newRate, float dt); 27 | 28 | void setAngle(float angle); // Used to set angle, this should be set as the starting angle 29 | float getRate(); // Return the unbiased rate 30 | 31 | /* These are used to tune the Kalman filter */ 32 | void setQangle(float Q_angle); 33 | /** 34 | * setQbias(float Q_bias) 35 | * Default value (0.003f) is in Kalman.cpp. 36 | * Raise this to follow input more closely, 37 | * lower this to smooth result of kalman filter. 38 | */ 39 | void setQbias(float Q_bias); 40 | void setRmeasure(float R_measure); 41 | 42 | float getQangle(); 43 | float getQbias(); 44 | float getRmeasure(); 45 | 46 | private: 47 | /* Kalman filter variables */ 48 | float Q_angle; // Process noise variance for the accelerometer 49 | float Q_bias; // Process noise variance for the gyro bias 50 | float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise 51 | 52 | float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector 53 | float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector 54 | float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate 55 | 56 | float P[2][2]; // Error covariance matrix - This is a 2x2 matrix 57 | }; 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 fungineers-youtube 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Onewheel_July.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Kalman.h" 3 | #include 4 | 5 | Servo HubMotor; 6 | Kalman kalmanX; // Create the Kalman instances 7 | Kalman kalmanY; 8 | 9 | /* IMU Data */ 10 | 11 | double accX, accY, accZ; 12 | double gyroX, gyroY, gyroZ; 13 | int16_t tempRaw; 14 | double dt=0; 15 | float angle_estimate=0; 16 | double throttle=1500; //initial value of throttle to the motors 17 | double gyroXangle, gyroYangle; // Angle calculate using the gyro only 18 | double compAngleX, compAngleY; // Calculated angle using a complementary filter 19 | double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter 20 | float angle; 21 | float CorrectedAngle; //initial 0 value for footpad safety 22 | float PWMangle; 23 | float PWMout; 24 | float VESCPWM, VESCPWMRawVal; 25 | uint32_t timer; 26 | uint8_t i2cData[14]; // Buffer for I2C data 27 | int count=0; //not really using this in the code. Should this be used? Maybe.. 28 | 29 | void setup() { 30 | Serial.begin(115200); //increase? 31 | Wire.begin(); 32 | #if ARDUINO >= 157 33 | Wire.setClock(400000UL); // Set I2C frequency to 400kHz 34 | #else 35 | TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz 36 | #endif 37 | 38 | i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz 39 | i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling 40 | i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s 41 | i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g 42 | while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once 43 | while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode 44 | 45 | while (i2cRead(0x75, i2cData, 1)); 46 | if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register 47 | Serial.print(F("Error reading sensor")); 48 | while (1); 49 | } 50 | HubMotor.attach(3); //attatch the Hub motor to pin 3. This is from the servo library. Attach is needed before running 51 | HubMotor.writeMicroseconds(1500); //Initial pwm data sent to the wheel. 1500 = zero throttle. Range 1000 - 2000 52 | delay(100); // Wait for sensor to stabilize 53 | 54 | while (i2cRead(0x3B, i2cData, 6)); 55 | accX = (int16_t)((i2cData[0] << 8) | i2cData[1]); 56 | accY = (int16_t)((i2cData[2] << 8) | i2cData[3]); 57 | accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]); 58 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 59 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 60 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 61 | #else // Eq. 28 and 29 62 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 63 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 64 | #endif 65 | 66 | kalmanX.setAngle(roll); // Set starting angle 67 | kalmanY.setAngle(pitch); 68 | gyroXangle = roll; 69 | gyroYangle = pitch; 70 | compAngleX = roll; 71 | compAngleY = pitch; 72 | 73 | timer = micros(); 74 | } 75 | 76 | void loop() { 77 | 78 | /* Foot Button */ 79 | 80 | double footpad1 = analogRead(A6); //A6 and A7 pins are reading the voltage from the footpad voltage divider 81 | double footpad2 = analogRead(A7); 82 | 83 | while (i2cRead(0x3B, i2cData, 14)); 84 | accX = (int16_t)((i2cData[0] << 8) | i2cData[1]); 85 | accY = (int16_t)((i2cData[2] << 8) | i2cData[3]); 86 | accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]); 87 | tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]); 88 | gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]); 89 | gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]); 90 | gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]); 91 | 92 | dt = (double)(micros() - timer) / 1000000; // Calculate delta time 93 | timer = micros(); 94 | 95 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 96 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 97 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 98 | #else // Eq. 28 and 29 99 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 100 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 101 | #endif 102 | 103 | double gyroXrate = gyroX / 131.0; // Convert to deg/s 104 | double gyroYrate = gyroY / 131.0; // Convert to deg/s 105 | 106 | #ifdef RESTRICT_PITCH 107 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 108 | if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { 109 | kalmanX.setAngle(roll); 110 | compAngleX = roll; 111 | kalAngleX = roll; 112 | gyroXangle = roll; 113 | } else 114 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 115 | 116 | if (abs(kalAngleX) > 90) 117 | gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading 118 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); 119 | #else 120 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 121 | if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { 122 | kalmanY.setAngle(pitch); 123 | compAngleY = pitch; 124 | kalAngleY = pitch; 125 | gyroYangle = pitch; 126 | } else 127 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter 128 | 129 | if (abs(kalAngleY) > 90) 130 | gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading 131 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 132 | #endif 133 | 134 | gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter 135 | gyroYangle += gyroYrate * dt; 136 | //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate 137 | //gyroYangle += kalmanY.getRate() * dt; 138 | 139 | compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter 140 | compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; 141 | 142 | // Reset the gyro angle when it has drifted too much 143 | if (gyroXangle < -180 || gyroXangle > 180) 144 | gyroXangle = kalAngleX; 145 | if (gyroYangle < -180 || gyroYangle > 180) 146 | gyroYangle = kalAngleY; 147 | 148 | angle = -kalAngleX; //angle estimate to use. For some reason the calculated angle is inverted. I might be using the gyro wrong, but this is how i prefer. 149 | Serial.println(angle); 150 | 151 | 152 | //Serial.println("Original Angle:"); 153 | //Serial.println(angle); //remove in final code 154 | //CorrectedAngle = angle*15 + 1500; 155 | 156 | CorrectedAngle = map (angle, -30, 30, 0, 255); //mapping the angle from -30 to 30, to 1000 to 2000. i.e. making it from an angle in degrees to PWM output from 1000 to 2000 157 | VESCPWM = constrain (CorrectedAngle, 0, 255); // make sure the angle is with the defined constraints. Anything crazy outside the lmits will be constrained by this command 158 | //PWMangle = map (angle, -30, 30, 0, 255); 159 | //PWMout = constrain (PWMangle, 0, 255); 160 | //Serial.println (PWMangle); 161 | //Main command to write to vesc if footpads are active 162 | 163 | //do { 164 | //HubMotor.writeMicroseconds(VESCPWM); //sending pwm to the vesc.. 165 | analogWrite (VESCPWM, 3); 166 | analogWrite (VESCPWM, A6); 167 | // Serial.println("Corrected Angle: " + String(CorrectedAngle)); //remove all prints before running. 168 | // Serial.println("Output: "); 169 | Serial.println(VESCPWM); 170 | // Serial.println ("footpadV" ); 171 | // Serial.println(footpad1); 172 | delay(10); 173 | } 174 | //while ((footpadV1 > 1000) && (footpadV2 > 1000)); //if both footpads are pressed. 175 | // } 176 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DIY-Onewheel-Arduino-Code 2 | Arduino code for the DIY Onewheel (from the Fungineers Youtube channel). 3 | Project Brief: 4 | The project is to build a replica of the Onewheel at 1/4 the cost. 5 | Parts: Hub motor wheel, Vesc, Arduino, MPU6050, 10s2p battery (or whatever you like). Rails and enclosures (3D printed or aluminium, or a mixture of both). 6 | About the code: 7 | The Arduino code is a working one. 8 | It takes the angle measurements from the Gyro (MPU), and converts them to a pwm signal between 1000 and 2000 and sends to the VESC. 9 | It uses the Kalman filter and the complemenary filter. The servo and the wire libraries are used as well. 10 | Future plans/ collaboration ideas: 11 | Make the code UART instead of just PWM so that the UART port on the VESC can be used (to send and recevie data), and display the received date on a screen via bluetooth module. 12 | -------------------------------------------------------------------------------- /UARTcontrol: -------------------------------------------------------------------------------- 1 | /* 2 | An alternative to PWM/PPM control of the VESC is UART. 3 | This code covers control of UART via Arduino using the awesome VESCUART library by Solidgeek. 4 | This is not final but a work in progress. 5 | Created by: Mohammad from youtube.com/Fungineers for the DIY OneWheel Project 6 | */ 7 | 8 | #include "MPU9250.h" 9 | #include 10 | #include "datatypes.h" 11 | #include "buffer.h" 12 | #include "crc.h" 13 | 14 | /** Initiate VescUart class */ 15 | VescUart UART; 16 | /** Initiate MPU class */ 17 | MPU9250 mpu; 18 | int status; 19 | int16_t ax, ay, az; //acc readings 20 | int16_t gx, gy, gz; //gyro readings 21 | float duty; 22 | float current; 23 | bool rideron; 24 | double footpadV1 = analogRead(A2); //A2 and A7 pins are reading the voltage from the footpad voltage divider 25 | double footpadV2 = analogRead(A7); 26 | double roll = -mpu.getRoll(); 27 | //PID stuff 28 | double PID, Kp = 0.7 , Ki = 0, Kd = 0.00; 29 | 30 | void setup() { 31 | Serial.begin(9600); 32 | Wire.begin(); 33 | mpu.setup(); 34 | /** Setup UART port (Serial1 on Atmega324) */ 35 | Serial1.begin(15200); 36 | while (!Serial1) {} 37 | if (status < 0) { 38 | Serial.println("IMU initialization unsuccessful"); 39 | Serial.println("Check IMU wiring or try cycling power"); 40 | Serial.print("Status: "); 41 | Serial.println(status); 42 | while (1) {} 43 | } 44 | UART.setSerialPort(&Serial1);//define which port to use as UART 45 | } 46 | 47 | void loop() { 48 | 49 | static uint32_t prev_ms = millis(); 50 | mpu.update(); 51 | double roll = -mpu.getRoll(); 52 | if (((millis() - prev_ms) > 10) && (footpadV2 > 120) && ((rideron == true) || (roll < 2 && roll > -2))) //10ms is the refresh time. 100hz. 53 | { 54 | mpu.update(); 55 | double roll = mpu.getRoll(); 56 | Serial.print("roll "); Serial.println (roll); 57 | gx = -(mpu.getGyro(1)/131.0); //get the change in rate of roll. 58 | Serial.println("Gyro "); Serial.println (gx); 59 | PID = Kp * (roll) + Kd * gx; //calculate PIDs 60 | Serial.print("PID "); Serial.println (PID); 61 | duty = mapf (PID, -12, 12, -1, 1.0); //mapping the angle from -12 to 12, to Duty cycle, i.e. -1 to 1 to send to vesc. 62 | duty = constrain (duty, -1.0, 1.0); 63 | Serial.print("duty "); Serial.println (duty); 64 | /*******************************************Sending data to VESC***************************************/ 65 | //UART.setCurrent(duty); //sending current to VESC via UART. 66 | UART.setDuty(duty); //sendign duty to VESC via UART 67 | /*******************************************Receiving data to VESC***************************************/ 68 | Serial.print("rpm "); 69 | Serial.print(UART.data.rpm); 70 | Serial.println("Vin "); 71 | Serial.print(UART.data.inpVoltage); 72 | Serial.print("aH "); 73 | Serial.println(UART.data.ampHours); 74 | Serial.print("Tach "); 75 | Serial.println(UART.data.tachometerAbs); 76 | rideron = true; //rideron is set to true after the first start and remains true. 77 | prev_ms = millis(); 78 | } 79 | else 80 | { 81 | rideron = false; 82 | UART.setDuty(0); //send zero throttle 83 | //UART.setCurrent(0); //send zero throttle 84 | } 85 | } 86 | /************************************mapf function (to map angle values to duty/current output values**********************************************/ 87 | 88 | float mapf(double val, double in_min, double in_max, double out_min, double out_max) 89 | { 90 | return (val - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 91 | } 92 | -------------------------------------------------------------------------------- /Wire.h: -------------------------------------------------------------------------------- 1 | /* 2 | TwoWire.h - TWI/I2C library for Arduino & Wiring 3 | Copyright (c) 2006 Nicholas Zambetti. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts 20 | */ 21 | 22 | #ifndef TwoWire_h 23 | #define TwoWire_h 24 | 25 | #if defined(__IMXRT1052__) || defined(__IMXRT1062__) 26 | #include "WireIMXRT.h" 27 | 28 | #elif defined(__arm__) && defined(TEENSYDUINO) 29 | #include "WireKinetis.h" 30 | 31 | #elif defined(__AVR__) 32 | 33 | #include 34 | #include 35 | 36 | #define BUFFER_LENGTH 32 37 | #define WIRE_HAS_END 1 38 | 39 | class TwoWire : public Stream 40 | { 41 | private: 42 | static uint8_t rxBuffer[]; 43 | static uint8_t rxBufferIndex; 44 | static uint8_t rxBufferLength; 45 | 46 | static uint8_t txAddress; 47 | static uint8_t txBuffer[]; 48 | static uint8_t txBufferIndex; 49 | static uint8_t txBufferLength; 50 | 51 | static uint8_t transmitting; 52 | static void onRequestService(void); 53 | static void onReceiveService(uint8_t*, int); 54 | static void (*user_onRequest)(void); 55 | static void (*user_onReceive)(int); 56 | static void sda_rising_isr(void); 57 | public: 58 | TwoWire(); 59 | void begin(); 60 | void begin(uint8_t); 61 | void begin(int); 62 | void end(); 63 | void setClock(uint32_t); 64 | void setSDA(uint8_t); 65 | void setSCL(uint8_t); 66 | void beginTransmission(uint8_t); 67 | void beginTransmission(int); 68 | uint8_t endTransmission(void); 69 | uint8_t endTransmission(uint8_t); 70 | uint8_t requestFrom(uint8_t, uint8_t); 71 | uint8_t requestFrom(uint8_t, uint8_t, uint8_t); 72 | uint8_t requestFrom(int, int); 73 | uint8_t requestFrom(int, int, int); 74 | uint8_t requestFrom(uint8_t, uint8_t, uint32_t, uint8_t, uint8_t); 75 | virtual size_t write(uint8_t); 76 | virtual size_t write(const uint8_t *, size_t); 77 | virtual int available(void); 78 | virtual int read(void); 79 | virtual int peek(void); 80 | virtual void flush(void); 81 | void onReceive( void (*)(int) ); 82 | void onRequest( void (*)(void) ); 83 | 84 | #ifdef CORE_TEENSY 85 | // added by Teensyduino installer, for compatibility 86 | // with pre-1.0 sketches and libraries 87 | void send(uint8_t b) { write(b); } 88 | void send(uint8_t *s, uint8_t n) { write(s, n); } 89 | void send(int n) { write((uint8_t)n); } 90 | void send(char *s) { write(s); } 91 | uint8_t receive(void) { 92 | int c = read(); 93 | if (c < 0) return 0; 94 | return c; 95 | } 96 | #endif 97 | inline size_t write(unsigned long n) { return write((uint8_t)n); } 98 | inline size_t write(long n) { return write((uint8_t)n); } 99 | inline size_t write(unsigned int n) { return write((uint8_t)n); } 100 | inline size_t write(int n) { return write((uint8_t)n); } 101 | using Print::write; 102 | }; 103 | 104 | extern TwoWire Wire; 105 | 106 | #endif 107 | #endif 108 | --------------------------------------------------------------------------------