/* Catheter Rotation Measuring Device Program Author: Linna Ma 2 * Sensors: Adafruit BNO055 IMU sensor AS5600 Magectic Encoder 1 * Screen: Adafruit SSD1306 OLED 4 * LEDs 1 * stepper motor Date: Spring 2023 */ // libraries #include #include // For magectic encoder #include // For IMU Sensor #include // For screen #include // For IMU Sensor #include // For IMU Sensor #include // For good measure #include // for inverse tangent function // define OLED screen size #define OLED_WIDTH 128 #define OLED_HEIGHT 64 #define OLED_ADDR 0x3C // define LED pins #define LED_HANDLE_RED 12 #define LED_HANDLE_GREEN 11 #define LED_TIP_RED 10 #define LED_TIP_GREEN 13 // define object AS5600 ams5600; // define magectic encoder Adafruit_BNO055 myIMU = Adafruit_BNO055(); // define an object IMU Adafruit_SSD1306 display(OLED_WIDTH, OLED_HEIGHT); // define OLED // define stepper motor params int halfsteps[8] = {5,4,6,2,10,8,9,1}; //clockwise turn 0.9 deg / step int halfsteps_ccw[8] = {1,9,8,10,2,6,4,5};//ccw turn 0.9 deg / step int i,j = 0; int button = 2; unsigned long timer ; float roll, pitch, yaw = 0; //float magVal = 0; // functions void welcomeMessage(); void displayAngle(String(roll),String(magVal)); void LED(float roll,float magVal); // ********************************************************************* void setup() { Serial.begin(9600); Wire.begin(); // button input setup pinMode(button, INPUT); digitalWrite(button, HIGH); //stepper motor input setup DDRA = 0xFF; // LED set up for signaling pinMode(LED_HANDLE_RED, OUTPUT); pinMode(LED_HANDLE_GREEN, OUTPUT); //pinMode(LED_TIP_RED, OUTPUT); //pinMode(LED_TIP_GREEN, OUTPUT); // OLED set up and display MASACath logo display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR); welcomeMessage(); // IMU initialization myIMU.begin(); // get temperature from the IMU chip int8_t temp = myIMU.getTemp(); //Serial.print("temp: "); //Serial.print(temp); //Serial.println(" deg C"); myIMU.setExtCrystalUse(true); //get IMU calibration (gryo: sit still, acc: move and hold, mg: swing) and angle data uint8_t system, gyro, accel, mg = 0; myIMU.getCalibration(&system,&gyro,&accel,&mg); delay(2000); //Serial.println("initializing"); imu::Quaternion quat_ini = myIMU.getQuat(); float rollOffset = atan2(2*(quat_ini.w()*quat_ini.x()+quat_ini.y()*quat_ini.z()),1-2*(quat_ini.x()*quat_ini.x()+quat_ini.y()*quat_ini.y()))*180/3.1415926/0.9; while (rollOffset < 0 && i<=-rollOffset){ PORTA = halfsteps[i%8]; delay(10); i++; } while (rollOffset >= 0 && i<=rollOffset){ PORTA = halfsteps_ccw[i%8]; delay(10); i++; } //Serial.println("Handle sensor calibration done."); delay(2000); ams5600.begin(); ams5600.setMaxAngle(360); float degrees = ams5600.rawAngle() * 0.087912; ams5600.setOffset(-degrees); //Serial.println("Tip sensor calibration done."); } // ********************************************************************* void loop(){ //uint8_t system, gyro, accel, mg = 0; float magVal=0; int flag = 0; imu::Quaternion quat = myIMU.getQuat(); roll = atan2(2*(quat.w()*quat.x()+quat.y()*quat.z()),1-2*(quat.x()*quat.x()+quat.y()*quat.y()))*180/3.1415926; magVal = ams5600.rawAngle() * 0.087912; if (roll < 0){ roll = roll + 360; } displayAngle(String(roll),String(magVal)); LED(roll,magVal); if (digitalRead(button)==LOW){ //Serial.println("Button is pressed"); flag = 1; } timer = millis(); unsigned long elapsed = timer / 1000; if (flag == 1){ // motor run cw 90 deg while (j<=100){ PORTA = halfsteps[j%8]; delay(10); j++; //Serial.print(elapsed); //Serial.print(","); imu::Quaternion quat = myIMU.getQuat(); roll = atan2(2*(quat.w()*quat.x()+quat.y()*quat.z()),1-2*(quat.x()*quat.x()+quat.y()*quat.y()))*180/3.1415926; //Serial.print(roll); //Serial.print(","); /* get magetic encoder data: Raw data reports 0 - 4095 segments, which is 0.087 of a degree 360 / 4095*/ magVal = ams5600.rawAngle() * 0.087912; //Serial.println(0.9*j); Serial.println(magVal); //displayAngle(String(roll),String(magVal)); delay(100); } } } void welcomeMessage(){ display.clearDisplay(); display.setTextColor(WHITE); display.setCursor(20,32); display.setTextSize(2); display.println("MASACath"); //Show the name,you can remove it or replace it display.display(); delay(2000); display.clearDisplay(); //display.setTextSize(2); //display.setTextColor(WHITE); //display.setCursor(0, 0); //display.println("MASACath"); // detect magect and display magect message display.setTextSize(1); display.setTextColor(WHITE); display.setCursor(0, 17); // magnet detection if(ams5600.detectMagnet() == 0 ){ while(1){ if(ams5600.detectMagnet() == 1 ){ //Serial.print("Current Magnitude: "); //Serial.println(ams5600.readMagnitude()); display.println("detect magnet"); display.display(); delay(1000); break; } else{ display.println("Can not detect magnet"); display.display(); } delay(2000); } } } void displayAngle(String(roll),String(magVal)){ display.clearDisplay(); display.setTextSize(2); display.setTextColor(WHITE); display.setCursor(0, 0); display.println("MASACath"); display.setTextSize(1); display.setTextColor(WHITE); display.setCursor(0, 20); display.println("Handle Angle: "); display.setTextSize(1); display.setTextColor(WHITE); display.setCursor(0, 40); display.println("Tip Angle: "); // ********************************************************************* display.setTextSize(1); display.setTextColor(WHITE); display.setCursor(40, 30); display.print(String(roll)); display.println(" deg"); display.display(); display.setTextSize(1); display.setTextColor(WHITE); if(ams5600.detectMagnet() == 0 ){ while(1){ if(ams5600.detectMagnet() == 1 ){ break; } else{ display.setCursor(0, 50); display.println("Can not detect magnet"); display.display(); } delay(100); } } display.setCursor(40, 50); display.print(String(magVal)); display.println(" deg"); display.display(); delay(1000); } void LED(float roll,float magVal){ // LED flash when IMU calibrated if (roll < 0){ digitalWrite(LED_HANDLE_RED, HIGH); digitalWrite(LED_HANDLE_GREEN, LOW); } else if (roll >= 0 && roll <= 2 && magVal >= 0 && magVal <= 2){ digitalWrite(LED_HANDLE_RED, LOW); digitalWrite(LED_HANDLE_GREEN, HIGH); } else {//if (roll > 2 && magVal > 2){ digitalWrite(LED_HANDLE_RED, HIGH); digitalWrite(LED_HANDLE_GREEN, LOW); } //LED flash when magectic encoder calibrated //if (magVal > 1){ // digitalWrite(LED_TIP_RED, HIGH); // digitalWrite(LED_TIP_GREEN, LOW); //} //else if (magVal >= 0 && magVal <= 1){ // digitalWrite(LED_TIP_RED, LOW); // digitalWrite(LED_TIP_GREEN, HIGH); //} } //pitch = -asin(2*(quat.w()*quat.y()-quat.z()*quat.x())); //yaw = -atan2(2*(quat.w()*quat.z()+quat.x()*quat.y()),1-2*(quat.y()*quat.y()+quat.z()*quat.z()))- 3.1415926/2; // ********************************************************************* //Serial.print(quat.w()); //Serial.print("t"); //Serial.print(quat.x()); //Serial.print("t"); //Serial.print(quat.y()); //Serial.print("t"); //Serial.println(quat.z());