TreppenRobo
Prototyp V2
Dependencies: PM2_Libary
Diff: main.cpp
- Branch:
- michi
- Revision:
- 53:42b3280e4510
- Parent:
- 50:058dc65d0fa4
- Parent:
- 52:adfcbf71be5b
- Child:
- 55:8cb262e56efb
--- a/main.cpp Tue Apr 19 20:35:29 2022 +0200
+++ b/main.cpp Wed Apr 20 08:36:27 2022 +0200
@@ -2,6 +2,7 @@
#include "PM2_Libary.h"
#include <cmath>
#include <cstdint>
+#include "mapping.cpp"
#include <cstdio>
#include "math.h"
//*******************************************************************************************************************************************************************
@@ -31,6 +32,7 @@
// 78:1, 100:1, ... Metal Gearmotor 20Dx44L mm 12V CB
DigitalOut enable_motors(PB_15); // create DigitalOut object to enable dc motors
float pwm_period_s = 0.00005f; // define pwm period time in seconds and create FastPWM objects to command dc motors
+
//motor pin declaration
FastPWM pwm_M_right (PB_13); //motor pin decalaration for wheels right side
FastPWM pwm_M_left (PA_9); //motor pin decalaration for wheels left side
@@ -51,7 +53,6 @@
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
-
//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
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
@@ -75,9 +76,14 @@
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);
+extern double mapping (float adc_value_mV);
+
// calculates the deg which the arm has to take to reach a certain height (the input height has to be the height of OK Gripper area)
double calc_arm_deg_for_height(int height_mm)
{
+ float deg_arm;
if ((height_mm - dist_arm_ground - (dist_grappleratt_grappler_uk - gripper_area_height)) > arm_length) //check if height is reachable
{
printf("Error in calc_arm_deg_for_height: desired height is bigger than Wall-E arm lenght."); // error message when desired height is not reachable.
@@ -85,10 +91,9 @@
else
{
float height_arm = height_mm - dist_arm_ground - (dist_grappleratt_grappler_uk - gripper_area_height); // calculates the height which only the arm has to cover (- attachement height (arm to robot) etc.)
- float deg_arm = asin(height_arm / arm_length) * 180.0/pi; // calculates the absolute degrees which the arm has to reach
- return deg_arm;
+ deg_arm = asin(height_arm / arm_length) * 180.0/pi; // calculates the absolute degrees which the arm has to reach
}
- return NULL; // <------ maybe error testing necessary (value deg_arm might not be returned)
+ return deg_arm;
}
//calculates the deg which the wheels have to turn in order to cover specified distance in mm
@@ -113,7 +118,6 @@
}
//Drives forward into the next step
-// calculatioin of acctual distance with wheels is needed
int drive_straight(float distance)
{
float deg_to_turn = wheel_dist_to_deg(distance);
@@ -139,47 +143,6 @@
return NULL;
}
-//pow function is here so we dont have to use the math.h library ************* unnecessary math.h is used any way ***************
-//it takes 2 arguments the base can be any negative or positive floating point number the power has to be a hos to be an "integer" defined as a double
-double powerx(double base, double pow2)
-{
- double result = -1;
- double power = pow2;
- double basis = base;
- result = 1;
- //handling negative exponents
- if(power<0)
- {
- for(double i=1; i<=(power*(-1.0)); i++)
- {
- result *= basis;
- }
- result = 1.0/result;
- }
- //handling positive exponents
- else
- {
- for(double i=1; i<=power; i++)
- {
- result *= basis;
- }
- }
- return result;
-}
-
-double mapping(float adc_value_mV)
-{
- double distance = 0.0f; //distance in mm
- double infY =360 , supY = 2360; //Window for sensor values
- double voltage_mV = adc_value_mV;
- double p1 = -1.127*powerx(10,-14), p2 = 8.881*powerx(10,-11), p3 = -2.76*powerx(10,-7), p4 = 0.0004262, p5 = -0.3363, p6 = 120.1 ; //faktoren fuer polynomkurve -> von matlab exportiert
- if((voltage_mV > infY) && (voltage_mV < supY))
- {
- distance = p1*powerx(voltage_mV,5) + p2*powerx(voltage_mV,4) + p3*powerx(voltage_mV,3) + p4*powerx(voltage_mV,2) + p5*voltage_mV + p6;
- }
- return (distance);
-}
-
// while loop gets executed every main_task_period_ms milliseconds
int main_task_period_ms = 30; // define main task period time in ms e.g. 30 ms -> main task runns ~33,33 times per second
Timer main_task_timer; // create Timer object which we use to run the main task every main task period time in ms
@@ -190,7 +153,7 @@
// attach button fall and rise functions to user button object
user_button.fall(&user_button_pressed_fcn);
user_button.rise(&user_button_released_fcn);
-
+
while (true)
{
enable_motors = 1;
@@ -198,8 +161,6 @@
switch (ToNextFunction)
{
- case 0:
- break;
case 1:
start_position();
@@ -235,7 +196,7 @@
// read timer and make the main thread sleep for the remaining time span (non blocking)
int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();
thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms);
- return 0;
+ return 0;
}