40 uint8_t temp = TCCR3B, temp1 = TCCR1B;
41 TCCR3B &= ~(1 << CS30);
42 TCCR1B &= ~(1 << CS10);
53 void egoShield::setup(uint16_t acc, uint16_t vel,
float P,
float I,
float D,
float res, int8_t stallsens, uint16_t shutterDelay)
55 #ifdef ARDUINO_AVR_USTEPPER_S
59 #ifdef ARDUINO_AVR_USTEPPER_S_LITE
82 #ifdef ARDUINO_AVR_USTEPPER_S
83 stepper.setMaxVelocity(this->
velocity/16.0);
85 stepper.setMaxVelocity(this->
velocity);
89 #ifdef ARDUINO_AVR_USTEPPER_S
90 stepper.setup(PID,200);
91 stepper.checkOrientation(20.0);
98 #ifdef ARDUINO_AVR_USTEPPER_S
101 stepper.moveToEnd(CCW, map(stallsens, -64, 63, 0.0, 1.0));
104 stepper.encoder.setHome();
106 pinMode(
FWBT ,INPUT);
107 pinMode(
PLBT ,INPUT);
108 pinMode(
RECBT ,INPUT);
109 pinMode(
BWBT ,INPUT);
110 pinMode(
OPTO,OUTPUT);
111 digitalWrite(
OPTO ,HIGH);
112 digitalWrite(
FWBT ,HIGH);
113 digitalWrite(
PLBT ,HIGH);
114 digitalWrite(
RECBT ,HIGH);
115 digitalWrite(
BWBT ,HIGH);
119 stepper.disablePid();
121 stepper.encoder.setHome();
125 stepper.moveAngle(30);
126 while(stepper.getMotorState());
128 stepper.encoder.setHome();
129 stepper.setControlThreshold(10);
130 stepper.disablePid();
132 setPoint = stepper.encoder.getAngleMoved();
136 TIMSK4 = (1 << OCIE4A);
137 TCCR4A = (1 << WGM41);
138 TCCR4B = (1 << WGM42) | (1 << WGM43) | ( 1 << CS41);
142 void egoShieldTimeLapse::loop(
void)
144 setPoint = stepper.encoder.getAngleMoved();
166 void egoShieldTeach::loop(
void)
168 setPoint = stepper.encoder.getAngleMoved();
191 static bool continousForward = 0;
192 static bool continousBackwards = 0;
198 if(this->forwardBtn.
state != HOLD)
202 continousForward = 0;
205 else if(continousBackwards)
207 if(this->backwardsBtn.
state != HOLD)
211 continousBackwards = 0;
214 if(this->forwardBtn.
btn)
216 if(this->forwardBtn.
state == HOLD)
218 if(!continousForward)
220 stepper.runContinous(CCW);
221 continousForward = 1;
223 this->forwardBtn.
btn = 0;
227 stepper.moveAngle(-5.0);
228 this->forwardBtn.
btn = 0;
231 else if(this->backwardsBtn.
btn)
233 if(this->backwardsBtn.
state == HOLD)
235 if(!continousBackwards)
237 stepper.runContinous(CW);
238 continousBackwards = 1;
240 this->backwardsBtn.
btn = 0;
244 stepper.moveAngle(5.0);
245 this->backwardsBtn.
btn = 0;
248 else if(this->recordBtn.
btn == 1)
250 this->recordBtn.
btn = 0;
268 else if(this->playBtn.
btn == 1)
274 continousForward = 0;
275 continousBackwards = 0;
276 stepper.moveToAngle(
pos[0]);
281 while(this->playBtn.
state == HOLD);
288 static bool continousForward = 0;
289 static bool continousBackwards = 0;
294 if(this->forwardBtn.
state != HOLD)
298 continousForward = 0;
299 stepper.moveAngle(0.01);
302 else if(continousBackwards)
304 if(this->backwardsBtn.
state != HOLD)
308 continousBackwards = 0;
309 stepper.moveAngle(-0.01);
312 if(this->playBtn.
btn)
314 while(this->playBtn.
state == PRESSED);
315 if(this->playBtn.
state == DEPRESSED)
318 continousForward = 0;
319 continousBackwards = 0;
329 stepper.moveAngle(0.01);
334 stepper.disablePid();
338 while(this->playBtn.
state == HOLD);
342 else if(this->forwardBtn.
btn)
344 if(this->forwardBtn.
state == HOLD)
346 if(!continousForward)
348 stepper.runContinous(CCW);
349 continousForward = 1;
351 this->forwardBtn.
btn = 0;
355 stepper.moveAngle(-5.0);
356 this->forwardBtn.
btn = 0;
359 else if(this->backwardsBtn.
btn)
361 if(this->backwardsBtn.
state == HOLD)
363 if(!continousBackwards)
365 stepper.runContinous(CW);
366 continousBackwards = 1;
368 this->backwardsBtn.
btn = 0;
372 stepper.moveAngle(5.0);
373 this->backwardsBtn.
btn = 0;
376 else if(this->recordBtn.
btn == 1)
379 continousForward = 0;
380 continousBackwards = 0;
387 static uint8_t started = 0;
388 static bool lastMove = 0;
392 if(this->recordBtn.
btn)
394 while(this->recordBtn.
state == PRESSED);
395 if(this->recordBtn.
state == DEPRESSED)
407 else if(this->playBtn.
btn)
409 while(this->playBtn.
state == PRESSED);
410 if(this->playBtn.
state == DEPRESSED)
421 stepper.moveToAngle(
pos[0]);
426 while(this->playBtn.
state == HOLD);
433 #ifdef ARDUINO_AVR_USTEPPER_S
434 if(!stepper.driver.readRegister(VACTUAL))
436 if(!stepper.getMotorState())
467 if(this->forwardBtn.
state == HOLD)
472 else if(this->backwardsBtn.
state == HOLD)
484 if(this->forwardBtn.
btn == 1 && this->velocity <= 9900 && this->acceleration <= 19900)
486 this->forwardBtn.
btn = 0;
490 else if(this->backwardsBtn.
btn == 1 && this->velocity >= 10 && this->acceleration >= 10)
492 this->backwardsBtn.
btn = 0;
496 else if(this->playBtn.
btn == 1)
498 #ifdef ARDUINO_AVR_USTEPPER_S
499 stepper.setMaxVelocity(this->
velocity/16.0);
501 stepper.setMaxVelocity(this->
velocity);
513 if(this->playBtn.
btn)
515 while(this->playBtn.
state == PRESSED);
516 if(this->playBtn.
state == DEPRESSED)
525 while(this->playBtn.
state == HOLD);
530 if(this->forwardBtn.
state == HOLD)
535 else if(this->backwardsBtn.
state == HOLD)
544 static uint8_t step = 0;
545 static uint32_t i = 0, j = 0;
546 static uint8_t runState = 0;
548 stepper.getMotorState();
553 digitalWrite(
OPTO, HIGH);
556 this->forwardBtn.
btn = 0;
559 else if(this->backwardsBtn.
btn == 1 &&
stepSize >= 0.5)
561 this->backwardsBtn.
btn = 0;
564 else if(this->recordBtn.
btn == 1)
566 this->recordBtn.
btn = 0;
574 this->forwardBtn.
btn = 0;
577 else if(this->backwardsBtn.
btn == 1 &&
interval >= (500 + this->shutterDelay))
579 this->backwardsBtn.
btn = 0;
582 else if(this->recordBtn.
btn == 1)
585 this->recordBtn.
btn = 0;
591 if(this->playBtn.
btn)
593 while(this->playBtn.
state == PRESSED);
594 if(this->playBtn.
state == DEPRESSED)
612 OLD=stepper.encoder.getAngleMoved();
620 else if(runState == 1)
622 if(!stepper.getMotorState())
629 else if(runState == 2)
633 digitalWrite(
OPTO, LOW);
639 else if(runState == 3)
641 if(((millis() - j) > 200))
643 digitalWrite(
OPTO, HIGH);
648 else if(runState == 4)
656 if(this->playBtn.
btn == 1)
663 #ifdef ARDUINO_AVR_USTEPPER_S
664 if(!stepper.getMotorState() && (
stepSize*
resolution*0.9>abs(OLD-stepper.encoder.getAngleMoved())))
670 stepper.encoder.setHome();
671 stepper.moveToAngle(30);
672 while(stepper.getMotorState());
673 stepper.encoder.setHome();
682 if(stepper.isStalled())
684 stepper.moveToEnd(CCW);
685 stepper.encoder.setHome();
686 stepper.moveToAngle(30);
687 while(stepper.getMotorState());
688 stepper.encoder.setHome();
701 this->debounce(&forwardBtn,(PINB >> 5) & 0x01);
702 this->debounce(&playBtn,(PINB >> 3) & 0x01);
703 this->debounce(&recordBtn,(PIND >> 2) & 0x01);
704 this->debounce(&backwardsBtn,(PINB >> 4) & 0x01);
709 this->
screen->clrScreen();
710 this->
screen->drawImage(logoBmp, 10, 0, 112, 48);
715 static int32_t position;
716 static bool lastPidMode;
722 this->
screen->clrScreen();
723 lastPidMode = pidMode;
725 position = (int32_t)(stepper.encoder.getAngleMoved()/this->
resolution);
729 sBuf.toCharArray(buf, 20);
730 this->
screen->drawRect(0,0,127,7,1);
731 this->
screen->drawRect(0,48,127,63,1);
732 this->
screen->printString(
"Idle",2,0,1);
735 this->
screen->printString(
"PID ON ",45,0,1);
739 this->
screen->printString(
"PID OFF",45,0,1);
741 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
742 this->
screen->drawImage(playBmp, 28, 48, 16, 16, 1);
743 this->
screen->drawImage(stopBmp, 39, 48, 16, 16, 1);
744 this->
screen->drawImage(recordBmp, 67, 48, 16, 16, 1);
745 this->
screen->drawImage(pauseBmp, 81, 48, 16, 16, 1);
746 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
747 sBuf.toCharArray(buf, 20);
748 this->
screen->printString((
const uint8_t*)buf,2,24,0);
752 if((int32_t)(stepper.encoder.getAngleMoved()/this->
resolution) != position)
754 position = (int32_t)(stepper.encoder.getAngleMoved()/this->
resolution);
757 sBuf.toCharArray(buf, 20);
759 this->
screen->drawRect(62,24,127,31,0);
760 this->
screen->printString((
const uint8_t*)buf,62,24,0);
763 if(pidMode != lastPidMode)
765 lastPidMode = pidMode;
768 this->
screen->printString(
"PID ON ",45,0,1);
772 this->
screen->printString(
"PID OFF",45,0,1);
780 static int32_t position;
781 static bool lastPidMode, updatePosition;
787 this->
screen->clrScreen();
788 lastPidMode = pidMode;
794 sBuf.toCharArray(buf, 20);
795 this->
screen->drawRect(0,0,127,7,1);
796 this->
screen->drawRect(0,48,127,63,1);
799 this->
screen->printString(
"PID ON ",45,0,1);
803 this->
screen->printString(
"PID OFF",45,0,1);
805 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
806 this->
screen->drawImage(stopBmp, 35, 48, 16, 16, 1);
807 this->
screen->drawImage(recordBmp, 73, 48, 16, 16, 1);
808 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
815 sBuf.toCharArray(buf, 22);
816 this->
screen->printString((
const uint8_t*)buf,2,24,0);
824 sBuf.toCharArray(buf, 22);
825 this->
screen->printString((
const uint8_t*)buf,2,24,0);
836 sBuf.toCharArray(buf, 22);
837 this->
screen->drawRect(0,24,127,31,0);
838 this->
screen->printString((
const uint8_t*)buf,2,24,0);
843 if((int32_t)(
pos/this->
resolution) != position || updatePosition)
850 sBuf.toCharArray(buf, 20);
852 this->
screen->drawRect(62,24,127,31,0);
853 this->
screen->printString((
const uint8_t*)buf,62,24,0);
857 if(pidMode != lastPidMode)
859 this->
screen->drawRect(0,0,127,7,1);
860 lastPidMode = pidMode;
863 this->
screen->printString(
"PID ON ",45,0,1);
867 this->
screen->printString(
"PID OFF",45,0,1);
875 static int32_t position;
876 static bool lastPidMode, lastMode, lastLoopMode, lastIndex;
877 static float lastVelocity;
883 this->
screen->clrScreen();
884 lastPidMode = pidMode;
886 this->
screen->drawRect(0,0,127,7,1);
887 this->
screen->drawRect(0,48,127,63,1);
888 this->
screen->printString(
"Play",2,0,1);
891 this->
screen->printString(
"PID ON ",45,0,1);
895 this->
screen->printString(
"PID OFF",45,0,1);
897 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
898 this->
screen->drawImage(playBmp, 34, 48, 16, 16, 1);
899 this->
screen->drawImage(stopBmp, 48, 48, 16, 16, 1);
900 this->
screen->drawImage(pauseBmp, 79, 48, 16, 16, 1);
901 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
907 this->
screen->drawImage(repeatBmp, 112, 0, 16, 8, 1);
913 this->
screen->printString(
"Adjust velocity",2,24,0);
917 this->
screen->printString(
"Moving to pos",2,24,0);
918 String(index).toCharArray(buf, 5);
919 this->
screen->printString((
const uint8_t*)buf,90,24,0);
922 this->
screen->printString(
"Speed: ",2,32,0);
923 String(this->
velocity).toCharArray(buf, 5);
924 this->
screen->printString((
const uint8_t*)buf,60,32,0);
930 String(this->
velocity).toCharArray(buf, 5);
931 this->
screen->printString((
const uint8_t*)buf,60,32,0);
940 this->
screen->drawRect(0,24,127,31,0);
941 this->
screen->printString(
"Adjust velocity",2,24,0);
948 this->
screen->drawRect(0,24,127,31,0);
949 this->
screen->printString(
"Moving to pos",2,24,0);
950 String(index).toCharArray(buf, 5);
951 this->
screen->printString((
const uint8_t*)buf,90,24,0);
955 if(lastIndex != index)
957 String(index).toCharArray(buf, 5);
958 this->
screen->printString((
const uint8_t*)buf,90,24,0);
968 this->
screen->drawRect(110,0,127,7,1);
971 this->
screen->drawImage(repeatBmp, 112, 0, 16, 8, 1);
976 if(pidMode != lastPidMode)
978 this->
screen->drawRect(0,0,127,7,1);
979 lastPidMode = pidMode;
982 this->
screen->printString(
"PID ON ",45,0,1);
986 this->
screen->printString(
"PID OFF",45,0,1);
994 static bool lastPidMode, lastLoopMode;
1000 lastPidMode = pidMode;
1002 this->
screen->clrScreen();
1003 this->
screen->drawRect(0,0,127,7,1);
1004 this->
screen->drawRect(0,48,127,63,1);
1005 this->
screen->printString(
"Pause",2,0,1);
1008 this->
screen->printString(
"PID ON ",45,0,1);
1012 this->
screen->printString(
"PID OFF",45,0,1);
1014 this->
screen->drawImage(playBmp, 22, 48, 16, 16, 1);
1015 this->
screen->drawImage(stopBmp, 35, 48, 16, 16, 1);
1021 this->
screen->drawImage(repeatBmp, 112, 0, 16, 8, 1);
1024 this->
screen->printString(
"Paused at Position ",2,24,0);
1025 String(index).toCharArray(buf, 5);
1026 this->
screen->printString((
const uint8_t*)buf,114,24,0);
1032 this->
screen->drawRect(110,0,127,7,1);
1035 this->
screen->drawImage(repeatBmp, 112, 0, 16, 8, 1);
1041 if(pidMode != lastPidMode)
1043 this->
screen->drawRect(45,0,127,7,1);
1044 lastPidMode = pidMode;
1047 this->
screen->printString(
"PID ON ",45,0,1);
1051 this->
screen->printString(
"PID OFF",45,0,1);
1061 static bool lastPidMode;
1062 static uint8_t lastStep;
1063 static float lastStepSize, lastInterval;
1065 static int32_t lastAngle;
1071 lastPidMode = pidMode;
1073 this->
screen->clrScreen();
1074 this->
screen->drawRect(0,0,127,7,1);
1075 this->
screen->drawRect(0,48,127,63,1);
1076 this->
screen->printString(
"Time",2,0,1);
1081 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
1082 this->
screen->drawImage(recordBmp, 73, 48, 16, 16, 1);
1083 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
1084 this->
screen->printString(
"<-",115,24,0);
1088 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
1089 this->
screen->drawImage(recordBmp, 73, 48, 16, 16, 1);
1090 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
1091 this->
screen->printString(
"<-",115,32,0);
1095 this->
screen->drawImage(playBmp, 28, 48, 16, 16, 1);
1096 this->
screen->drawImage(stopBmp, 35, 48, 16, 16, 1);
1100 this->
screen->drawImage(stopBmp, 35, 48, 16, 16, 1);
1104 this->
screen->drawRect(45,0,127,7,1);
1107 this->
screen->printString(
"PID ON ",45,0,1);
1111 this->
screen->printString(
"PID OFF",45,0,1);
1114 angle = (int32_t)(stepper.encoder.getAngleMoved()/
resolution);
1116 sBuf =
"Stepsize: ";
1120 sBuf.toCharArray(buf, 22);
1121 this->
screen->printString((
const uint8_t*)buf,2,24,0);
1122 sBuf =
"Interval: ";
1126 sBuf.toCharArray(buf, 22);
1127 this->
screen->printString((
const uint8_t*)buf,2,32,0);
1132 sBuf.toCharArray(buf, 22);
1133 this->
screen->printString((
const uint8_t*)buf,2,40,0);
1137 angle = (int32_t)(stepper.encoder.getAngleMoved()/
resolution);
1144 sBuf.toCharArray(buf, 22);
1145 this->
screen->drawRect(66,24,127,31,0);
1146 this->
screen->printString((
const uint8_t*)buf,68,24,0);
1154 sBuf.toCharArray(buf, 22);
1155 this->
screen->drawRect(66,32,127,39,0);
1156 this->
screen->printString((
const uint8_t*)buf,68,32,0);
1159 if(lastAngle != angle)
1164 sBuf.toCharArray(buf, 22);
1165 this->
screen->drawRect(68,40,127,47,0);
1166 this->
screen->printString((
const uint8_t*)buf,68,40,0);
1169 if(step != lastStep)
1171 this->
screen->drawRect(0,48,127,63,1);
1172 this->
screen->drawRect(115,24,127,39,0);
1175 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
1176 this->
screen->drawImage(recordBmp, 73, 48, 16, 16, 1);
1177 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
1178 this->
screen->printString(
"<-",115,24,0);
1182 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
1183 this->
screen->drawImage(recordBmp, 73, 48, 16, 16, 1);
1184 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
1185 this->
screen->printString(
"<-",115,32,0);
1189 this->
screen->drawImage(playBmp, 35, 48, 16, 16, 1);
1193 this->
screen->drawImage(stopBmp, 35, 48, 16, 16, 1);
1199 if(pidMode != lastPidMode)
1201 this->
screen->drawRect(45,0,127,7,1);
1202 lastPidMode = pidMode;
1205 this->
screen->printString(
"PID ON ",45,0,1);
1209 this->
screen->printString(
"PID OFF",45,0,1);
1215 void egoShield::debounce(
buttons *btn, uint8_t sample)
1217 if(btn->
state == DEPRESSED)
1221 if( (btn->
debounce & 0x1F) == 0x00)
1223 btn->
state = PRESSED;
1232 else if((btn->
state == PRESSED) || (btn->
state == HOLD))
1236 if(btn->
state != HOLD)
1248 if( (btn->
debounce & 0x1F) == 0x1F)
1250 btn->
state = DEPRESSED;
1259 if(btn->
state == HOLD)
1261 if(btn->
time == HOLDTICK)
1277 btn->
state = DEPRESSED;