Diff: main.cpp

Revision:

36:c6961428c1e1

Parent:

34:4c04c6c04c55

Child:

37:6ac4db3cc57b

--- a/main.cpp	Wed Mar 30 11:10:11 2022 +0200
+++ b/main.cpp	Wed Mar 30 11:22:42 2022 +0200
@@ -12,41 +12,43 @@
 #include "PM2_Libary.h"
 
 //------------------------------------general IO and Variables----------------------------------------------------------
-//arbitrary allocation of pins -> for testing this has to be adjusted
-AnalogIn ch0(PA_15); //IR TOF sensor at the front
-AnalogIn RearSensor(PC_9); //IR TOF sensor at the back facing down
 
-DigitalOut armMotor(PC_12);
-// main() runs in its own thread in the OS
-    // Enable the ADC, 12MHz,  the final term should be (2**BURST_CHANNELS - 1)
 int ToNextFunction;
 float trigerValueRearSensor = 0.0f;
 float trigerValueFrontSensor = 0.0f;
 
 
 //-----------------------------------------actors Definition----------------------------------------------------
+// Infrarot sensors pin declaration
+AnalogIn FrontSensor(PC_6); //IR TOF sensor at the front facing down
+AnalogIn RearSensor(PC_8); //IR TOF sensor at the back facing down
+AnalogIn LeftEyeSensor(PC_2); // Infrared sensor on top in "head" part left eye
+AnalogIn RightEyeSensor(PC_3); // Infrared sensor on top in "head" part right eye
 
-//Encoder pin 
-EncoderCounter  encoder_M_right(PA_0, PA_1);
-EncoderCounter  encoder_M_left(PA_2, PA_3);  //Pin zuweisung Provisorisch!!!
+//Encoder pin declaration
+EncoderCounter  encoder_M_right(PA_6, PC_7); //encoder pin decalaration for wheels right side
+EncoderCounter  encoder_M_left(PB_6, PB_7); //encoder pin decalaration for wheels left side
+EncoderCounter encoder_M_arm(PA_0, PA_1); //encoder pin decalaration for arm
 
-//motor pin
-FastPWM pwm_M_right(PA_10);              
-FastPWM pwm_M_left(PA_9); 
+//motor pin declaration
+FastPWM pwm_M_right(PA_9);              
+FastPWM pwm_M_left(PB_13);
+FastPWM pwm_M_arm(PA_10);
 
 // create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
 float max_voltage = 12.0f;                  // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
-float counts_per_turn = 20.0f * 78.125f;    // define counts per turn at gearbox end: counts/turn * gearratio
+float counts_per_turn_wheels = 2000.0f * 100.0f;    // define counts per turn at gearbox end (counts/turn * gearratio) for wheels
+float counts_per_turn_arm = 40000.0f * 100.0f;      // define counts per turn at gearbox end (counts/turn * gearratio) for arm
 float kn = 180.0f / 12.0f;                  // define motor constant in rpm per V
 float k_gear = 100.0f / 78.125f;            // define additional ratio in case you are using a dc motor with a different gear box, e.g. 100:1
 float kp = 0.1f;                            // define custom kp, this is the default speed controller gain for gear box 78.125:1
 
 //motors for tracks
-PositionController positionController_M_right(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_right, encoder_M_right); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
-PositionController positionController_M_left(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_left, encoder_M_left); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
+PositionController positionController_M_right(counts_per_turn_wheels * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_right, encoder_M_right); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
+PositionController positionController_M_left(counts_per_turn_wheels * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_left, encoder_M_left); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
 
 //Arm Motor
-PositionController positionController_M_Arm(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_right, encoder_M_right); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
+PositionController positionController_M_Arm(counts_per_turn_arm * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M_arm, encoder_M_arm); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
 
 //-----------------------------------------Functions----------------------------------------------------------
 //only moves the arm in to the starting position