1
unknown
c_cpp
a month ago
12 kB
4
Indexable
#include <SimpleFOC.h> #define LASERSENSOR_PIN 14 BLDCMotor motor1 = BLDCMotor(11, 5.6, 120); //5.6, 120 BLDCMotor motor2 = BLDCMotor(14, 32, 9); //32, 9 float filmTension = 1; //film tension in (N) float init_Spool1D = 80.52; //80.52 - DimaSpool / 64.75 - ReelSpool float init_Spool2D = 30.2; //30.8 float fScanSpeed = 2; // rad/sec const int R_value = 100; //R_Color 220 Mono 100 const int G_value = 180; //G_Color 100 Mono 180 const int B_value = 230; //B_Color 200 Mono 230 long frameCount = 1; float scannerState = 0; float lastState = -1; float motor1Target; float motor2Target; // Переменные для отладочного вывода unsigned long lastDebugTime = 0; const unsigned long debugInterval = 120; // Интервал вывода отладочной информации (мс) bool stateDebug = false; unsigned long stopLastTime = 0; unsigned long moveInterval = 165; // Интервал движения мотора перед сенсором (мс) 175 const unsigned long stopInterval = 300; // Интервал остановки мотора (мс) 800 const unsigned long filmSettleInterval = 300; bool move = false; unsigned long ledCamLastTime = 0; unsigned long ledCamInterval = stopInterval; //Интервал переключения между R/G/B (мс) int ledCamMode = 0; //Режим работы камеры и светодиодов bool ledCamState = false; //Блок работы камеры (ON/OFF) float target1 = .5; float target2 = 0.13; volatile bool sensorStopState = false; //Interrupt unsigned long sensorInterval = 250; //Interrupt void setup() { Serial.begin(115200); initMotors(); initLaser(); initCam(); initLED(); delay(1000); float m1InitTorque = tensionM1TargetSpoolD(filmTension/2); float m2InitTorque = tensionM2TargetSpoolD(filmTension/2); initialTension(m1InitTorque, m2InitTorque, 3000); //Init tension } void loop() { //ON STATE SWITCH if (scannerState != lastState && ledCamState == false) { //motorRewind(false); switch(int(scannerState)) { case 0: // PAUSE MODE Serial.println("PAUSE MODE"); LED_Control(R_value, G_value, B_value); motorsStandbyTorque(); break; case 1: // SCAN MODE Serial.println("SCAN MODE"); //motorsStartMoveSimple(); motorsStartMoveSpoolD(); break; case 2: // LED OFF / PAUSE MODE Serial.println("SCAN STOP MODE"); LED_Control(0, 0, 0); motorsStandbyTorque(); break; case 3: //RADIUS RESET frameCount = 0; Serial.println("Framecount Reset / Please specify new m1D & m2D"); requestRadi(); resetSpoolD(); scannerState = 0; //EXIT break; case 4: // REWIND Serial.println("REWIND"); //motorsRewindSimple(true, 10); break; case 5: // WIND FORWARD //motorsRewindSimple(false, 10); break; case 6: //TEST MODE motor1Target = -tensionM1TargetSpoolD(filmTension/2); motor2.controller = MotionControlType::velocity; motor2Target = .5; break; } lastState = scannerState; } //STATE CALL switch(int(scannerState)) { case 0: break; case 1: // SCAN MODE sensorCall(); //OLD SENSOR CALL //if (sensorStopState) sensorCallInterrupt(); //Interrupt FLAG //sensorInterval = target1; //led_cam_call_BW(true); break; case 3: //SensorSetup sensorDebug(); break; case 6: //TEST if(sensorStopState) { frameCount++; Serial.println("Perf CATCH! Frame: " + String(frameCount)); sensorStopState = false; } //Serial.println(" Val: " + String(analogRead(LASERSENSOR_PIN))); //delay(500); break; } //if(frameCount == 50+1) scannerState = 0; //led_cam_call_Color(); //led_cam_call_BWONLY(); led_cam_call_BW(true); camTriggerOffCall(); motorLoop(); callCommand(); } void led_cam_call_BWONLY() { unsigned long currentTime = millis(); if (ledCamState) { switch (ledCamMode) { case 0: //WAIT FOR FILM TO SETTLE / LED ON ledCamLastTime = currentTime; ledCamMode++; LED_Control(R_value,G_value,B_value); if(stateDebug) Serial.println("WAIT / LED ON: " + String(ledCamLastTime)); break; //WAITING case 1: //CAM TRIGGER 1 if (currentTime - ledCamLastTime >= filmSettleInterval) { ledCamLastTime = currentTime; ledCamMode++; Serial.println("Prepare to CamTrigger"); camTriggerON(); if(stateDebug) Serial.println("Frame Capture / CAM TRIGGER: " + String(ledCamLastTime)); } break; //CAM TAKING PICTURE R case 2: //CAM FALLS TRIGGER if (currentTime - ledCamLastTime >= 100) { ledCamLastTime = currentTime; ledCamMode++; LED_Control(0,0,0); camTriggerON(); if(stateDebug) Serial.println("FALLSFRAME 1 / CAM TRIGGER: " + String(ledCamLastTime)); } break; //CAM TAKING PICTURE G(0) case 3: //CAM FALLS TRIGGER if (currentTime - ledCamLastTime >= 100) { ledCamLastTime = currentTime; ledCamMode++; camTriggerON(); if(stateDebug) Serial.println("FALLSFRAME 2 / CAM TRIGGER: " + String(ledCamLastTime)); } break; //CAM TAKING PICTURE B(0) case 4: //EXIT if (currentTime - ledCamLastTime >= 100) { ledCamLastTime = currentTime; ledCamMode = 0; ledCamState = false; frameCount++; if(stateDebug) { Serial.println("Frame: " + String(frameCount) + " / MOVE: " + String(ledCamLastTime)); Serial.println("///////////////////////////////"); } filmLenghtCall(); motorsStartMoveSpoolD(); torqueControlDebug(); } break; } } } void led_cam_call_BW (bool hdr) { unsigned long currentTime = millis(); if (ledCamState) { switch (ledCamMode) { case 0: //WAIT FOR FILM TO SETTLE / LED ON ledCamLastTime = currentTime; ledCamMode++; LED_Control(R_value,G_value,B_value); Serial.println("///////////////////////////////"); Serial.println("Frame: " + String(frameCount) + " / MOVE: " + String(ledCamLastTime)); if(stateDebug) Serial.println("WAIT / LED ON: " + String(ledCamLastTime)); break; //WAITING case 1: //CAM TRIGGER 1 if (currentTime - ledCamLastTime >= filmSettleInterval) { ledCamLastTime = currentTime; if(!hdr) ledCamMode = 4; else ledCamMode++; camTriggerON(); if(stateDebug) Serial.println("Frame Capture / CAM TRIGGER: " + String(ledCamLastTime)); } break; //CAM TAKING PICTURE 1 case 2: //CAM TRIGGER 2 if (currentTime - ledCamLastTime >= ledCamInterval/3) { ledCamLastTime = currentTime; ledCamMode++; camTriggerON(); if(stateDebug) Serial.println("Frame UnderExposure / CAM TRIGGER: " + String(ledCamLastTime)); } break; //CAM TAKING PICTURE 2 HDR case 3: //CAM TRIGGER 3 if (currentTime - ledCamLastTime >= ledCamInterval/3) { ledCamLastTime = currentTime; ledCamMode++; camTriggerON(); if(stateDebug) Serial.println("OverExposure/ CAM TRIGGER: " + String(ledCamLastTime)); } break; //CAM TAKING PICTURE 3 HDR case 4: //EXIT if (currentTime - ledCamLastTime >= ledCamInterval/3) { ledCamLastTime = currentTime; ledCamMode = 0; ledCamState = false; frameCount++; if(stateDebug) { } filmLenghtCall(); motorsStartMoveSpoolD(); torqueControlDebug(); } break; } } } void led_cam_call_Color() { unsigned long currentTime = millis(); if (ledCamState) { switch (ledCamMode) { case 0: //WAIT FOR FILM TO SETTLE / LED ON ledCamLastTime = currentTime; ledCamMode++; LED_Control(0, 0, 0); Serial.println("////////////////////////"); Serial.println("Frame: " + String(frameCount)); if(stateDebug) Serial.println("WAIT / LED ON: " + String(ledCamLastTime)); break; case 1: //CAM TRIGGER R / LED R if (currentTime - ledCamLastTime >= filmSettleInterval) { ledCamLastTime = currentTime; ledCamMode++; LED_Control(R_value, 0, 0); camTriggerON(); if(stateDebug) Serial.println("TRIGGER R: " + String(ledCamLastTime)); } break; //TAKING R CHANNEL case 2: //CAM TRIGGER G / LED G if (currentTime - ledCamLastTime >= ledCamInterval/3) { ledCamLastTime = currentTime; ledCamMode++; LED_Control(0, G_value, 0); camTriggerON(); if(stateDebug) Serial.println("TRIGGER G: " + String(ledCamLastTime)); } break; //TAKING G CHANNEL case 3: //CAM TRIGGER B / LED B if (currentTime - ledCamLastTime >= ledCamInterval/3) { ledCamLastTime = currentTime; ledCamMode++; LED_Control(0, 0, B_value); camTriggerON(); if(stateDebug) Serial.println("TRIGGER B: " + String(ledCamLastTime)); } break; //TAKING B CHANNEL case 4: //RADI CALC / LED OFF / MOVE if (currentTime - ledCamLastTime >= ledCamInterval/3) { ledCamLastTime = currentTime; ledCamMode = 0; ledCamState = false; LED_Control(0,0,0); frameCount++; //motorsStartMoveSimple(); filmLenghtCall(); motorsStartMoveSpoolD(); torqueControlDebug(); if(stateDebug) Serial.println("MOVE: " + String(ledCamLastTime)); } break; } } } bool firstValueReceived = false; void requestRadi() { Serial.println("Type TakeOUT Spool Rad: "); // Ждем и получаем первое значение while (!firstValueReceived) { if (Serial.available() > 0) { init_Spool1D = Serial.parseFloat(); // Очищаем буфер while (Serial.available()) { Serial.read(); } firstValueReceived = true; Serial.print("m1R: "); Serial.println(init_Spool1D, 2); // 6 знаков после запятой // Запрашиваем второе значение Serial.println("Type TakeIN Spool Rad: "); } delay(100); } // Ждем и получаем второе значение boolean secondValueReceived = false; while (!secondValueReceived) { if (Serial.available() > 0) { init_Spool2D = Serial.parseFloat(); secondValueReceived = true; Serial.print("m2R: "); Serial.println(init_Spool2D, 2); } delay(100); } firstValueReceived = false; }
Editor is loading...
Leave a Comment