Diff: main.cpp

Branch:

michi

Revision:

55:8cb262e56efb

Parent:

53:42b3280e4510

Child:

57:79732e5818d7

Child:

70:da5754e1514c

--- a/main.cpp	Wed Apr 20 08:36:27 2022 +0200
+++ b/main.cpp	Wed Apr 20 09:55:33 2022 +0200
@@ -7,15 +7,15 @@
 #include "math.h"
 //*******************************************************************************************************************************************************************
 // Defined Variables in mm coming from Hardware-team. Need to be updated
-float wheel_diameter                = 30; // diameter of wheel with caterpillar to calculate mm per wheel turn (4)
-float arm_length                    = 118.5; // lenght of arm from pivotpoint to pivotpoint (3)
-float dist_arm_attach_distsensor    = 20; // distance between pivot point arm  on body to start distancesensor on top in horizontal (6)
-float dist_distsensors              = 200; // distance between the two distancesensors on top of Wall-E (9)
-float dist_arm_ground               = 51; // distance between pivotpoint arm and ground (5)
-float gripper_area_height           = 16 ; // Height of Grappler cutout to grapple Stair (8)
-float dist_grappleratt_grappler_uk  = 33; // distance between pivotpoint Grappler and bottom edge (?)
+const float wheel_diameter                = 30; // diameter of wheel with caterpillar to calculate mm per wheel turn (4)
+const float arm_length                    = 118.5; // lenght of arm from pivotpoint to pivotpoint (3)
+const float dist_arm_attach_distsensor    = 20; // distance between pivot point arm  on body to start distancesensor on top in horizontal (6)
+const float dist_distsensors              = 200; // distance between the two distancesensors on top of Wall-E (9)
+const float dist_arm_ground               = 51; // distance between pivotpoint arm and ground (5)
+const float gripper_area_height           = 16 ; // Height of Grappler cutout to grapple Stair (8)
+const float dist_grappleratt_grappler_uk  = 33; // distance between pivotpoint Grappler and bottom edge (?)
 
-float height_stairs                 = 100; // height to top of next stairstep in mm
+const float height_stairs                 = 100; // height to top of next stairstep in mm
 //***********************************************************************************************************************************************************
 // declaration of Input - Output pins
 
@@ -27,7 +27,7 @@
 
 // Sharp GP2Y0A41SK0F, 4-40 cm IR Sensor
 float ir_distance_mV = 0.0f;    // define variable to store measurement from infrared distancesensor in mVolt
-AnalogIn ir_analog_in(PC_2);    // create AnalogIn object to read in infrared distance sensor, 0...3.3V are mapped to 0...1
+AnalogIn ir_analog_in(PC_3);    // create AnalogIn object to read in infrared distance sensor, 0...3.3V are mapped to 0...1
 
 // 78:1, 100:1, ... Metal Gearmotor 20Dx44L mm 12V CB
 DigitalOut enable_motors(PB_15);    // create DigitalOut object to enable dc motors
@@ -46,12 +46,12 @@
 // Hardware controll Setup and functions (motors and sensors)
 
 // 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_wheels    = 20.0f * 78.125f;    // define counts per turn at gearbox end (counts/turn * gearratio) for wheels
-float counts_per_turn_arm       = 20.0f * 78.125f * 10.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 (DC with 100:1 has 2'000 turns for 360°)
-float kp                        = 0.1f;     // define custom kp, this is the default speed controller gain for gear box 78.125:1
+const float max_voltage               = 12.0f;     // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
+const float counts_per_turn_wheels    = 20.0f * 78.125f;    // define counts per turn at gearbox end (counts/turn * gearratio) for wheels
+const float counts_per_turn_arm       = 20.0f * 78.125f * 10.0f;    // define counts per turn at gearbox end (counts/turn * gearratio) for arm
+const float kn                        = 180.0f / 12.0f;      // define motor constant in rpm per V
+const 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 (DC with 100:1 has 2'000 turns for 360°)
+const 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_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
@@ -65,16 +65,16 @@
 // logic functions for basic movement
 
 //placeholder variables for prototype testing
-int     drive_stright_mm = 100; // placeholder for testing drives amount forward
-int     drive_back_mm = -100; // placeholder for testing drives amount backwards
-int     ToNextFunction = 0;  // current state of the system (which function is beeing executed)
+const int   drive_stright_mm = 100; // placeholder for testing drives amount forward
+const int   drive_back_mm = -100; // placeholder for testing drives amount backwards
+int         ToNextFunction = 0;  // current state of the system (which function is beeing executed)
 
 // definition important variables
-float   pi = 2 * acos(0.0); // definiton of pi
-float   max_speed_rps_wheel = 0.6f;  // define maximum speed that the position controller is changig the speed for the wheels, has to be smaller or equal to kn * max_voltage
-float   max_speed_rps_arm = 0.3f; // define maximum speed that the position controller is changig the speed for the arm, has to be smaller or equal to kn * max_voltage
-float   start_deg_arm = -asin((dist_arm_ground - dist_grappleratt_grappler_uk) / arm_length) * 180.0/pi ; //calculates the starting degree of the arm (gripper has to touch ground in frotn of Wall-E)
-float   current_deg_arm = start_deg_arm; // saves the current degree the arm has.
+const float   pi = 2 * acos(0.0); // definiton of pi
+const float   max_speed_rps_wheel = 0.6f;  // define maximum speed that the position controller is changig the speed for the wheels, has to be smaller or equal to kn * max_voltage
+const float   max_speed_rps_arm = 0.3f; // define maximum speed that the position controller is changig the speed for the arm, has to be smaller or equal to kn * max_voltage
+float         start_deg_arm = -asin((dist_arm_ground - dist_grappleratt_grappler_uk) / arm_length) * 180.0/pi ; //calculates the starting degree of the arm (gripper has to touch ground in frotn of Wall-E)
+float         current_deg_arm = start_deg_arm; // saves the current degree the arm has.
 
 // import functions from file mapping
 extern double powerx(double base, double pow2);
@@ -117,7 +117,8 @@
     return NULL;
 }
 
-//Drives forward into the next step
+//Drives forward into the next step 
+//Prameter:distance in milimeter
 int drive_straight(float distance)
 {
     float deg_to_turn = wheel_dist_to_deg(distance);