Diff: main.cpp

Revision:

10:b8c60fd468f1

Parent:

9:6537eead1241

Child:

11:51ae2da8da55

--- a/main.cpp	Sat Oct 05 16:38:45 2019 +0000
+++ b/main.cpp	Sun Oct 06 13:14:40 2019 +0000
@@ -3,39 +3,42 @@
 #include "FastPWM.h"
 #include "QEI.h"
 
-Serial pc(USBTX, USBRX);         //verbinden met pc
+Serial pc(USBTX, USBRX);            //verbinden met pc
 DigitalOut motor2_direction(D4);    //verbinden met motor 2 op board (altijd d4)
 FastPWM motor2_pwm(D5);             //verbinden met motor 2 pwm (altijd d5)
-DigitalOut motor1_direction(D6);    //verbinden met motor 2 op board (altijd d4)
-FastPWM motor1_pwm(D7);             //verbinden met motor 2 pwm (altijd d5)
+DigitalOut motor1_direction(D6);    //verbinden met motor 2 op board (altijd d6)
+FastPWM motor1_pwm(D7);             //verbinden met motor 2 pwm (altijd d7)
 Ticker loop_ticker;                 //used in main()
 AnalogIn Pot1(A1);                  //pot 1 op biorobotics shield
 AnalogIn Pot2(A0);                  //pot 2 op biorobotics shield
-InterruptIn but1(D10);            //knop 1 op birorobotics shield
-InterruptIn but2(D9);            //knop 1 op birorobotics shield
+AnalogIn joystick_x(A2);            //input joystick
+AnalogIn joystick_y(A3);            //input joystick
+InterruptIn but1(D10);              //knop 1 op birorobotics shield
+InterruptIn but2(D9);               //knop 1 op birorobotics shield
 QEI encoder1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
-QEI encoder2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
+QEI encoder2 (D1,  D2,  NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
+
 
 //variables
 enum States {idle, cw, ccw, end, failure};
 States current_state;               //Definieren van de current_state met als keuzes Idle, cw, ccw, end en failure
 
 class motor_state {                 // Een emmer met variabelen die je hierna motor gaat noemen.
-    public:                         //Wat is public?
-    float pwm1; //pwm of 1st motor
-    float pwm2; //pwm of 2nd motor
-    int dir1; //direction of 1st motor
-    int dir2; //direction of 2nd motor
-    float speed1;   // speed motor 1
-    double speed2;   // speed motor 2
+    public:                         //variabelen kunnen worden gebruikt door externe functies
+    float pwm1;                     //pwm of 1st motor
+    float pwm2;                     //pwm of 2nd motor
+    int dir1;                       //direction of 1st motor
+    int dir2;                       //direction of 2nd motor
 };
-motor_state motor;              //Een emmer met variabelen met motor.iets als naam bijv. motor.speed1
+motor_state motor;                  //Een emmer met variabelen met motor.iets als naam bijv. motor.speed1
 
-bool state_changed = false;
+bool state_changed = false;         // wordt gebruikt in de state functies, om te kijken of de state net begint
 volatile bool but1_pressed = false;
 volatile bool but2_pressed = false;
-float pot_1 = 0;
-float pot_2 = 0;
+float pot_1;
+float pot_2;
+float x_input;
+float y_input;
 
 /*
 float x_movement = 0;
@@ -73,7 +76,9 @@
 
 // the funtions needed to run the program
 void measure_signals() {
-    
+    x_input = joystick_x.read();
+    y_input = joystick_y.read();
+
     /*
     letter = pc.getc()          // krijg de letter vanuit de keyboard
     if (letter == w){
@@ -109,7 +114,9 @@
     motor.speed1 = 0;
     motor.speed2 = 0;
     
-    if (but1_pressed == true) { //this moves the program from the idle to cw state
+    
+    //state guard
+    if (but1_pressed) { //this moves the program from the idle to cw state
         current_state = cw;
         state_changed = true; //to show next state it can initialize
         pc.printf("Changed state from idle to cw\r\n");
@@ -127,6 +134,9 @@
     motor.dir1 = 1;             //todo: check if this is actually clockwise
     motor.dir2 = 1;             //todo: check if this is actually clockwise
     
+    motor.pwm1 = x_input;       //nog niet echt de x, maar de rotatie, moet later met kinematics gefixt worden
+    motor.pwm2 = y_input;       // ook nog niet echt de y dus
+    //state guard
     if (!state_changed && but1_pressed) { //state wasnt just changed, button 1 has been pressed -> change state
         current_state = ccw;
         state_changed = true;
@@ -143,7 +153,11 @@
     }
     motor.dir1 = 0;
     motor.dir2 = 0;
-
+    
+    motor.pwm1 = x_input;       //nog niet echt de x
+    motor.pwm2 = y_input;       //nog niet echt de y
+    
+    //state guard
     if (!state_changed && but1_pressed) { //state niet veranderd, button 1 gepressd -> state verandert
         current_state = cw;
         state_changed = true;
@@ -152,15 +166,15 @@
 }
         
 void motor_controller() {
-    motor1_pwm.period(0.0000625‬f);              // 1/frequency van waarop hij draait
+
     motor1_direction = motor.dir1;              // Zet de richting goed
-    motor1_pwm.write(motor.speed1);             // Zet het op de snelheid van motor.speed1
+    motor1_pwm.write(motor.pwm1);             // Zet het op de snelheid van motor.speed1
     
-    motor2_pwm.period(0.0000625f);              // 1/frequency van waarop hij draait
+
     motor2_direction = motor.dir2;
-    motor2_pwm.write(motor.speed2);             
+    motor2_pwm.write(motor.pwm2);             
     return;
-    }
+}
     
 void output() {return;}
 
@@ -215,6 +229,8 @@
     
     motor.dir1 = 0;
     motor.dir2 = 0;
+    motor2_pwm.period(0.0000625f);              // 1/frequency van waarop hij draait
+    motor1_pwm.period(0.0000625f);              // 1/frequency van waarop hij draait
     
     but1.fall(&but1_interrupt);
     but2.fall(&but2_interrupt);