|
@@ -0,0 +1,86 @@
|
|
|
|
|
+package boebot;
|
|
|
|
|
+import stamp.core.*;
|
|
|
|
|
+
|
|
|
|
|
+public class MotorAansturing{
|
|
|
|
|
+ private int snelheidL;
|
|
|
|
|
+ private int snelheidR;
|
|
|
|
|
+ private boolean richting;
|
|
|
|
|
+ private PWM motorL, motorR;
|
|
|
|
|
+
|
|
|
|
|
+ public MotorAansturing(){
|
|
|
|
|
+ motorL = new PWM (CPU.pin13, 173, 2304); // motoren op de rem zetten
|
|
|
|
|
+ motorR = new PWM (CPU.pin12, 173, 2304);
|
|
|
|
|
+ }
|
|
|
|
|
+ public void setSnelheidL(int snelheid){
|
|
|
|
|
+ snelheidL = snelheid;
|
|
|
|
|
+ }
|
|
|
|
|
+ public void setSnelheidR(int snelheid){
|
|
|
|
|
+ snelheidR = snelheid;
|
|
|
|
|
+ }
|
|
|
|
|
+ public void setSnelheid(int snelheid){
|
|
|
|
|
+ setSnelheidR(snelheid);
|
|
|
|
|
+ setSnelheidL(snelheid);
|
|
|
|
|
+ }
|
|
|
|
|
+ public void setRichting(boolean r){
|
|
|
|
|
+ richting = r;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ public void start(){
|
|
|
|
|
+ int maxSnelheid = snelheidL;
|
|
|
|
|
+ System.out.println(maxSnelheid);
|
|
|
|
|
+ for (int i=0; i<=maxSnelheid; i = i + 10) {
|
|
|
|
|
+ setSnelheid(i);
|
|
|
|
|
+ rijden();
|
|
|
|
|
+ CPU.delay (2000);
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+ public void stop(){
|
|
|
|
|
+ int maxSnelheid = snelheidL;
|
|
|
|
|
+ for (int i=maxSnelheid; i>0; i = i - 10) {
|
|
|
|
|
+ setSnelheid(i);
|
|
|
|
|
+ rijden();
|
|
|
|
|
+ CPU.delay (2000);
|
|
|
|
|
+ }
|
|
|
|
|
+ motorL.update(173, 2304); // motoren op de rem zetten
|
|
|
|
|
+ motorR.update(173, 2304);
|
|
|
|
|
+ }
|
|
|
|
|
+ public void noodStop(){
|
|
|
|
|
+ motorL.update(173, 2304); // motoren op de rem zetten
|
|
|
|
|
+ motorR.update(173, 2304);
|
|
|
|
|
+ }
|
|
|
|
|
+ private void rijden()
|
|
|
|
|
+ {
|
|
|
|
|
+ int motorLWaarde =0;
|
|
|
|
|
+ int motorRWaarde =0;
|
|
|
|
|
+ if(richting){ //vooruit
|
|
|
|
|
+ motorLWaarde = 173-(23*snelheidL/100);
|
|
|
|
|
+ motorRWaarde = 173+(23*snelheidR/100);
|
|
|
|
|
+ }else if(!richting){ //achteruit
|
|
|
|
|
+ motorLWaarde = 173+(23*snelheidL/100);
|
|
|
|
|
+ motorRWaarde = 173-(23*snelheidR/100);
|
|
|
|
|
+ }
|
|
|
|
|
+ motorL.update(motorLWaarde, 2304); //waardes toepassen
|
|
|
|
|
+ motorR.update(motorRWaarde, 2304);
|
|
|
|
|
+ }
|
|
|
|
|
+ public void turn(int graden)
|
|
|
|
|
+ {
|
|
|
|
|
+ int factor = 0;
|
|
|
|
|
+ if(graden > 180 & graden <360){
|
|
|
|
|
+ motorL.update(150, 2304); // Linksom is de kortste route
|
|
|
|
|
+ motorR.update(150, 2304);
|
|
|
|
|
+ graden -= 180;
|
|
|
|
|
+
|
|
|
|
|
+ }else{
|
|
|
|
|
+ motorL.update(196, 2304); // rechtsom is de kortste route
|
|
|
|
|
+ motorR.update(196, 2304);
|
|
|
|
|
+ }
|
|
|
|
|
+ int delay =((37* graden) / 90)*10;
|
|
|
|
|
+ for (int i=0; i<10; i++) { // delay berekenen voor gegeven graades en toepasen
|
|
|
|
|
+ CPU.delay (delay);
|
|
|
|
|
+
|
|
|
|
|
+ }
|
|
|
|
|
+ motorL.update(173, 2304); // motoren afremmen en stil laten staan
|
|
|
|
|
+ motorR.update(173, 2304);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+}
|