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(10.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();
130 setPoint = stepper.encoder.getAngleMoved();
134 TIMSK4 = (1 << OCIE4A);
135 TCCR4A = (1 << WGM41);
136 TCCR4B = (1 << WGM42) | (1 << WGM43) | ( 1 << CS41);
139 void egoShieldTimeLapse::loop(
void)
141 setPoint = stepper.encoder.getAngleMoved();
163 void egoShieldTeach::loop(
void)
165 setPoint = stepper.encoder.getAngleMoved();
188 static bool continousForward = 0;
189 static bool continousBackwards = 0;
195 if(this->forwardBtn.
state != HOLD)
199 continousForward = 0;
202 else if(continousBackwards)
204 if(this->backwardsBtn.
state != HOLD)
208 continousBackwards = 0;
211 if(this->forwardBtn.
btn)
213 if(this->forwardBtn.
state == HOLD)
215 if(!continousForward)
217 stepper.runContinous(CCW);
218 continousForward = 1;
220 this->forwardBtn.
btn = 0;
224 stepper.moveAngle(-5.0);
225 this->forwardBtn.
btn = 0;
228 else if(this->backwardsBtn.
btn)
230 if(this->backwardsBtn.
state == HOLD)
232 if(!continousBackwards)
234 stepper.runContinous(CW);
235 continousBackwards = 1;
237 this->backwardsBtn.
btn = 0;
241 stepper.moveAngle(5.0);
242 this->backwardsBtn.
btn = 0;
245 else if(this->recordBtn.
btn == 1)
247 this->recordBtn.
btn = 0;
265 else if(this->playBtn.
btn == 1)
271 continousForward = 0;
272 continousBackwards = 0;
273 stepper.moveToAngle(
pos[0]);
278 while(this->playBtn.
state == HOLD);
285 static bool continousForward = 0;
286 static bool continousBackwards = 0;
291 if(this->forwardBtn.
state != HOLD)
295 continousForward = 0;
296 stepper.moveAngle(0.01);
299 else if(continousBackwards)
301 if(this->backwardsBtn.
state != HOLD)
305 continousBackwards = 0;
306 stepper.moveAngle(-0.01);
309 if(this->playBtn.
btn)
311 while(this->playBtn.
state == PRESSED);
312 if(this->playBtn.
state == DEPRESSED)
315 continousForward = 0;
316 continousBackwards = 0;
326 stepper.moveAngle(0.01);
331 stepper.disablePid();
335 while(this->playBtn.
state == HOLD);
339 else if(this->forwardBtn.
btn)
341 if(this->forwardBtn.
state == HOLD)
343 if(!continousForward)
345 stepper.runContinous(CCW);
346 continousForward = 1;
348 this->forwardBtn.
btn = 0;
352 stepper.moveAngle(-5.0);
353 this->forwardBtn.
btn = 0;
356 else if(this->backwardsBtn.
btn)
358 if(this->backwardsBtn.
state == HOLD)
360 if(!continousBackwards)
362 stepper.runContinous(CW);
363 continousBackwards = 1;
365 this->backwardsBtn.
btn = 0;
369 stepper.moveAngle(5.0);
370 this->backwardsBtn.
btn = 0;
373 else if(this->recordBtn.
btn == 1)
376 continousForward = 0;
377 continousBackwards = 0;
384 static uint8_t started = 0;
385 static bool lastMove = 0;
389 if(this->recordBtn.
btn)
391 while(this->recordBtn.
state == PRESSED);
392 if(this->recordBtn.
state == DEPRESSED)
404 else if(this->playBtn.
btn)
406 while(this->playBtn.
state == PRESSED);
407 if(this->playBtn.
state == DEPRESSED)
418 stepper.moveToAngle(
pos[0]);
423 while(this->playBtn.
state == HOLD);
430 #ifdef ARDUINO_AVR_USTEPPER_S
431 if(!stepper.driver.readRegister(VACTUAL))
433 if(!stepper.getMotorState())
464 if(this->forwardBtn.
state == HOLD)
469 else if(this->backwardsBtn.
state == HOLD)
481 if(this->forwardBtn.
btn == 1 && this->velocity <= 9900 && this->acceleration <= 19900)
483 this->forwardBtn.
btn = 0;
487 else if(this->backwardsBtn.
btn == 1 && this->velocity >= 10 && this->acceleration >= 10)
489 this->backwardsBtn.
btn = 0;
493 else if(this->playBtn.
btn == 1)
495 #ifdef ARDUINO_AVR_USTEPPER_S
496 stepper.setMaxVelocity(this->
velocity/16.0);
498 stepper.setMaxVelocity(this->
velocity);
510 if(this->playBtn.
btn)
512 while(this->playBtn.
state == PRESSED);
513 if(this->playBtn.
state == DEPRESSED)
522 while(this->playBtn.
state == HOLD);
527 if(this->forwardBtn.
state == HOLD)
532 else if(this->backwardsBtn.
state == HOLD)
541 static uint8_t step = 0;
542 static uint32_t i = 0, j = 0;
543 static uint8_t runState = 0;
545 stepper.disablePid();
546 stepper.getMotorState();
551 digitalWrite(
OPTO, HIGH);
554 this->forwardBtn.
btn = 0;
557 else if(this->backwardsBtn.
btn == 1 &&
stepSize >= 0.5)
559 this->backwardsBtn.
btn = 0;
562 else if(this->recordBtn.
btn == 1)
564 this->recordBtn.
btn = 0;
572 this->forwardBtn.
btn = 0;
575 else if(this->backwardsBtn.
btn == 1 &&
interval >= (500 + this->shutterDelay))
577 this->backwardsBtn.
btn = 0;
580 else if(this->recordBtn.
btn == 1)
583 this->recordBtn.
btn = 0;
589 if(this->playBtn.
btn)
591 while(this->playBtn.
state == PRESSED);
592 if(this->playBtn.
state == DEPRESSED)
610 OLD=stepper.encoder.getAngleMoved();
618 else if(runState == 1)
620 if(!stepper.getMotorState())
627 else if(runState == 2)
631 digitalWrite(
OPTO, LOW);
637 else if(runState == 3)
639 if(((millis() - j) > 200))
641 digitalWrite(
OPTO, HIGH);
646 else if(runState == 4)
654 if(this->playBtn.
btn == 1)
661 #ifdef ARDUINO_AVR_USTEPPER_S
662 if(!stepper.getMotorState() && (
stepSize*
resolution*0.9>abs(OLD-stepper.encoder.getAngleMoved())))
668 stepper.encoder.setHome();
669 stepper.moveToAngle(30);
670 while(stepper.getMotorState());
671 stepper.encoder.setHome();
680 if(stepper.isStalled())
682 stepper.moveToEnd(CCW);
683 stepper.encoder.setHome();
684 stepper.moveToAngle(30);
685 while(stepper.getMotorState());
686 stepper.encoder.setHome();
699 this->debounce(&forwardBtn,(PINB >> 5) & 0x01);
700 this->debounce(&playBtn,(PINB >> 3) & 0x01);
701 this->debounce(&recordBtn,(PIND >> 2) & 0x01);
702 this->debounce(&backwardsBtn,(PINB >> 4) & 0x01);
707 this->
screen->clrScreen();
708 this->
screen->drawImage(logoBmp, 10, 0, 112, 48);
713 static int32_t position;
714 static bool lastPidMode;
720 this->
screen->clrScreen();
721 lastPidMode = pidMode;
723 position = (int32_t)(stepper.encoder.getAngleMoved()/this->
resolution);
727 sBuf.toCharArray(buf, 20);
728 this->
screen->drawRect(0,0,127,7,1);
729 this->
screen->drawRect(0,48,127,63,1);
730 this->
screen->printString(
"Idle",2,0,1);
733 this->
screen->printString(
"PID ON ",45,0,1);
737 this->
screen->printString(
"PID OFF",45,0,1);
739 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
740 this->
screen->drawImage(playBmp, 28, 48, 16, 16, 1);
741 this->
screen->drawImage(stopBmp, 39, 48, 16, 16, 1);
742 this->
screen->drawImage(recordBmp, 67, 48, 16, 16, 1);
743 this->
screen->drawImage(pauseBmp, 81, 48, 16, 16, 1);
744 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
745 sBuf.toCharArray(buf, 20);
746 this->
screen->printString((
const uint8_t*)buf,2,24,0);
750 if((int32_t)(stepper.encoder.getAngleMoved()/this->
resolution) != position)
752 position = (int32_t)(stepper.encoder.getAngleMoved()/this->
resolution);
755 sBuf.toCharArray(buf, 20);
757 this->
screen->drawRect(62,24,100,31,0);
758 this->
screen->printString((
const uint8_t*)buf,62,24,0);
761 if(pidMode != lastPidMode)
763 lastPidMode = pidMode;
766 this->
screen->printString(
"PID ON ",45,0,1);
770 this->
screen->printString(
"PID OFF",45,0,1);
778 static int32_t position;
779 static bool lastPidMode, updatePosition;
785 this->
screen->clrScreen();
786 lastPidMode = pidMode;
792 sBuf.toCharArray(buf, 20);
793 this->
screen->drawRect(0,0,127,7,1);
794 this->
screen->drawRect(0,48,127,63,1);
797 this->
screen->printString(
"PID ON ",45,0,1);
801 this->
screen->printString(
"PID OFF",45,0,1);
803 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
804 this->
screen->drawImage(stopBmp, 35, 48, 16, 16, 1);
805 this->
screen->drawImage(recordBmp, 73, 48, 16, 16, 1);
806 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
813 sBuf.toCharArray(buf, 22);
814 this->
screen->printString((
const uint8_t*)buf,2,24,0);
822 sBuf.toCharArray(buf, 22);
823 this->
screen->printString((
const uint8_t*)buf,2,24,0);
834 sBuf.toCharArray(buf, 22);
835 this->
screen->drawRect(0,24,127,31,0);
836 this->
screen->printString((
const uint8_t*)buf,2,24,0);
841 if((int32_t)(
pos/this->
resolution) != position || updatePosition)
848 sBuf.toCharArray(buf, 20);
850 this->
screen->drawRect(62,24,127,31,0);
851 this->
screen->printString((
const uint8_t*)buf,62,24,0);
855 if(pidMode != lastPidMode)
857 this->
screen->drawRect(0,0,127,7,1);
858 lastPidMode = pidMode;
861 this->
screen->printString(
"PID ON ",45,0,1);
865 this->
screen->printString(
"PID OFF",45,0,1);
873 static int32_t position;
874 static bool lastPidMode, lastMode, lastLoopMode, lastIndex;
875 static float lastVelocity;
881 this->
screen->clrScreen();
882 lastPidMode = pidMode;
884 this->
screen->drawRect(0,0,127,7,1);
885 this->
screen->drawRect(0,48,127,63,1);
886 this->
screen->printString(
"Play",2,0,1);
889 this->
screen->printString(
"PID ON ",45,0,1);
893 this->
screen->printString(
"PID OFF",45,0,1);
895 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
896 this->
screen->drawImage(playBmp, 34, 48, 16, 16, 1);
897 this->
screen->drawImage(stopBmp, 48, 48, 16, 16, 1);
898 this->
screen->drawImage(pauseBmp, 79, 48, 16, 16, 1);
899 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
905 this->
screen->drawImage(repeatBmp, 112, 0, 16, 8, 1);
911 this->
screen->printString(
"Adjust velocity",2,24,0);
915 this->
screen->printString(
"Moving to pos",2,24,0);
916 String(index).toCharArray(buf, 5);
917 this->
screen->printString((
const uint8_t*)buf,90,24,0);
920 this->
screen->printString(
"Speed: ",2,32,0);
921 String(this->
velocity).toCharArray(buf, 5);
922 this->
screen->printString((
const uint8_t*)buf,60,32,0);
928 String(this->
velocity).toCharArray(buf, 5);
929 this->
screen->printString((
const uint8_t*)buf,60,32,0);
938 this->
screen->drawRect(0,24,127,31,0);
939 this->
screen->printString(
"Adjust velocity",2,24,0);
946 this->
screen->drawRect(0,24,127,31,0);
947 this->
screen->printString(
"Moving to pos",2,24,0);
948 String(index).toCharArray(buf, 5);
949 this->
screen->printString((
const uint8_t*)buf,90,24,0);
953 if(lastIndex != index)
955 String(index).toCharArray(buf, 5);
956 this->
screen->printString((
const uint8_t*)buf,90,24,0);
966 this->
screen->drawRect(110,0,127,7,1);
969 this->
screen->drawImage(repeatBmp, 112, 0, 16, 8, 1);
974 if(pidMode != lastPidMode)
976 this->
screen->drawRect(0,0,127,7,1);
977 lastPidMode = pidMode;
980 this->
screen->printString(
"PID ON ",45,0,1);
984 this->
screen->printString(
"PID OFF",45,0,1);
992 static bool lastPidMode, lastLoopMode;
998 lastPidMode = pidMode;
1000 this->
screen->clrScreen();
1001 this->
screen->drawRect(0,0,127,7,1);
1002 this->
screen->drawRect(0,48,127,63,1);
1003 this->
screen->printString(
"Pause",2,0,1);
1006 this->
screen->printString(
"PID ON ",45,0,1);
1010 this->
screen->printString(
"PID OFF",45,0,1);
1012 this->
screen->drawImage(playBmp, 22, 48, 16, 16, 1);
1013 this->
screen->drawImage(stopBmp, 35, 48, 16, 16, 1);
1019 this->
screen->drawImage(repeatBmp, 112, 0, 16, 8, 1);
1022 this->
screen->printString(
"Paused at Position ",2,24,0);
1023 String(index).toCharArray(buf, 5);
1024 this->
screen->printString((
const uint8_t*)buf,114,24,0);
1030 this->
screen->drawRect(110,0,127,7,1);
1033 this->
screen->drawImage(repeatBmp, 112, 0, 16, 8, 1);
1039 if(pidMode != lastPidMode)
1041 this->
screen->drawRect(45,0,127,7,1);
1042 lastPidMode = pidMode;
1045 this->
screen->printString(
"PID ON ",45,0,1);
1049 this->
screen->printString(
"PID OFF",45,0,1);
1059 static bool lastPidMode;
1060 static uint8_t lastStep;
1061 static float lastStepSize, lastInterval;
1063 static int32_t lastAngle;
1069 lastPidMode = pidMode;
1071 this->
screen->clrScreen();
1072 this->
screen->drawRect(0,0,127,7,1);
1073 this->
screen->drawRect(0,48,127,63,1);
1074 this->
screen->printString(
"Time",2,0,1);
1079 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
1080 this->
screen->drawImage(recordBmp, 73, 48, 16, 16, 1);
1081 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
1082 this->
screen->printString(
"<-",115,24,0);
1086 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
1087 this->
screen->drawImage(recordBmp, 73, 48, 16, 16, 1);
1088 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
1089 this->
screen->printString(
"<-",115,32,0);
1093 this->
screen->drawImage(playBmp, 28, 48, 16, 16, 1);
1094 this->
screen->drawImage(stopBmp, 35, 48, 16, 16, 1);
1098 this->
screen->drawImage(stopBmp, 35, 48, 16, 16, 1);
1102 this->
screen->drawRect(45,0,127,7,1);
1105 this->
screen->printString(
"PID ON ",45,0,1);
1109 this->
screen->printString(
"PID OFF",45,0,1);
1112 angle = (int32_t)(stepper.encoder.getAngleMoved()/
resolution);
1114 sBuf =
"Stepsize: ";
1118 sBuf.toCharArray(buf, 22);
1119 this->
screen->printString((
const uint8_t*)buf,2,24,0);
1120 sBuf =
"Interval: ";
1124 sBuf.toCharArray(buf, 22);
1125 this->
screen->printString((
const uint8_t*)buf,2,32,0);
1130 sBuf.toCharArray(buf, 22);
1131 this->
screen->printString((
const uint8_t*)buf,2,40,0);
1135 angle = (int32_t)(stepper.encoder.getAngleMoved()/
resolution);
1142 sBuf.toCharArray(buf, 22);
1143 this->
screen->drawRect(66,24,127,31,0);
1144 this->
screen->printString((
const uint8_t*)buf,68,24,0);
1152 sBuf.toCharArray(buf, 22);
1153 this->
screen->drawRect(66,32,127,39,0);
1154 this->
screen->printString((
const uint8_t*)buf,68,32,0);
1157 if(lastAngle != angle)
1162 sBuf.toCharArray(buf, 22);
1163 this->
screen->drawRect(68,40,114,47,0);
1164 this->
screen->printString((
const uint8_t*)buf,68,40,0);
1167 if(step != lastStep)
1169 this->
screen->drawRect(0,48,127,63,1);
1170 this->
screen->drawRect(115,24,127,39,0);
1173 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
1174 this->
screen->drawImage(recordBmp, 73, 48, 16, 16, 1);
1175 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
1176 this->
screen->printString(
"<-",115,24,0);
1180 this->
screen->drawImage(fastRewindBmp, 0, 48, 16, 16, 1);
1181 this->
screen->drawImage(recordBmp, 73, 48, 16, 16, 1);
1182 this->
screen->drawImage(fastForwardBmp, 112, 48, 16, 16, 1);
1183 this->
screen->printString(
"<-",115,32,0);
1187 this->
screen->drawImage(playBmp, 35, 48, 16, 16, 1);
1191 this->
screen->drawImage(stopBmp, 35, 48, 16, 16, 1);
1197 if(pidMode != lastPidMode)
1199 this->
screen->drawRect(45,0,127,7,1);
1200 lastPidMode = pidMode;
1203 this->
screen->printString(
"PID ON ",45,0,1);
1207 this->
screen->printString(
"PID OFF",45,0,1);
1213 void egoShield::debounce(
buttons *btn, uint8_t sample)
1215 if(btn->
state == DEPRESSED)
1219 if( (btn->
debounce & 0x1F) == 0x00)
1221 btn->
state = PRESSED;
1230 else if((btn->
state == PRESSED) || (btn->
state == HOLD))
1234 if(btn->
state != HOLD)
1246 if( (btn->
debounce & 0x1F) == 0x1F)
1248 btn->
state = DEPRESSED;
1257 if(btn->
state == HOLD)
1259 if(btn->
time == HOLDTICK)
1275 btn->
state = DEPRESSED;