TreppenRobo
Prototyp V2
Dependencies: PM2_Libary
Diff: main.cpp
- Branch:
- michi
- Revision:
- 43:7964411b4a6b
- Parent:
- 42:6e7ab1136354
- Child:
- 44:c2d4bc4be5f2
--- a/main.cpp Mon Apr 18 11:45:09 2022 +0200
+++ b/main.cpp Mon Apr 18 12:51:13 2022 +0200
@@ -4,7 +4,6 @@
#include <cstdint>
#include <cstdio>
#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)
@@ -16,7 +15,6 @@
float dist_grappleratt_grappler_uk = 33; // distance between pivotpoint Grappler and bottom edge (?)
float height_stairs = 100; // height to top of stairs in mm
-
//***********************************************************************************************************************************************************
// declaration of Input - Output pins
@@ -43,7 +41,7 @@
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
//***********************************************************************************************************************************************************
-// Hardware controll functions and setup
+// 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
@@ -61,10 +59,8 @@
// PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters
//PositionController positionController_M3(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M3, encoder_M3); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
-// LSM9DS1 IMU, carefull: not all PES boards have an imu (chip shortage)
-// LSM9DS1 imu(PC_9, PA_8); // create LSM9DS1 comunication object, if you want to be able to use the imu you need to #include "LSM9DS1_i2c.h"
-
//***********************************************************************************************************************************************************
+// logic functions for basic movement
//Platzhalter Variabeln für die Positionierung
float PositionStair = 0.2;
@@ -79,29 +75,38 @@
{
if ((height_mm - dist_arm_ground - (dist_grappleratt_grappler_uk - gripper_area_height)) > arm_length)
{
- printf("Error in calc_arm_deg_for_height: desireed height is bigger than Wall-E arm lenght."); // error message when desired height is not 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.
}
double height_arm = height_mm - dist_arm_ground - (dist_grappleratt_grappler_uk - gripper_area_height);
double deg_arm_rad = asin(height_mm / arm_length); // deg in radians
double pi = 2 * acos(0.0); // definiton of pi
- double deg_arm = deg_arm_rad * 180/pi; // deg in degrees
+ double deg_arm = deg_arm_rad * 180.0/pi; // deg in degrees
return deg_arm;
}
+// bring arme in starting position height of stairs.
int start_position()
{
- float deg = calc_arm_deg_for_height(height_stairs);
- if ((0 > deg) && (deg > 360))
+ float deg = calc_arm_deg_for_height(height_stairs); //deg which arm motor has to turn to in order to grab stair
+ if ((0.0 > deg) || (deg > 360.0))
{
- printf("degree is out of bound for Start Position.");
+ printf("Error in start_position: degree is out of bound for Start Position."); // error when desired reaching point is out of reach.
}
- positionController_M_Arm.setDesiredRotation(deg);
+ positionController_M_Arm.setDesiredRotation(deg); // command to turn motor to desired deg.
return NULL;
}
+//calculates the deg which the wheels have to turn in order to cover specified distnace in mm
+int wheel_dist_to_deg(int distance) // distance has to be in mm.
+{
+ double pi = 2 * acos(0.0); // definiton of pi
+ int deg_wheel = distance * 360 /(wheel_diameter * pi);
+ return deg_wheel;
+}
+
//Drives forward into the next step
// calculatioin of acctual distance with wheels is needed
-int drive(float distance)
+int drive_straight(float distance)
{
positionController_M_right.setDesiredRotation(distance,max_speed_rps);
positionController_M_left.setDesiredRotation(distance,max_speed_rps);
@@ -116,8 +121,16 @@
positionController_M_Arm.setDesiredRotation(deg);
return 0;
}
+//***********************************************************************************************************************************************************
-//pow function is here so we dont have to use the math.h library
+//Function which checks if sensors and motors have been wired correctly and the expectet results will happen. otherwise Wall-E will show with armmovement.
+int check_start()
+{
+
+ return 0;
+}
+
+//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)
{
@@ -164,17 +177,14 @@
// 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
-
-
+//***********************************************************************************************************************************************************
int main(void)
{
- // attach button fall and rise functions to user button object
+// 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;
@@ -186,7 +196,7 @@
printf("Case 1: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
// ToNextFunction+=1;
break;
- case 2: drive(PositionStair);
+ case 2: drive_straight(PositionStair);
printf("Case 2: Position Right(rot): %3.3f; Position Left (rot): %3.3f\n",
positionController_M_right.getRotation(),positionController_M_left.getRotation());
// ToNextFunction+=1;
@@ -195,7 +205,7 @@
// ToNextFunction+=1;
printf("Case 3: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
break;
- case 4: drive(PositionBackOff);
+ case 4: drive_straight(PositionBackOff);
printf("Case 4: Position Right(rot): %3.3f; Position Left (rot): %3.3f\n",
positionController_M_right.getRotation(),positionController_M_left.getRotation());
// ToNextFunction+=1;
@@ -220,10 +230,13 @@
user_button_timer.reset();
}
-void user_button_released_fcn() {
+void user_button_released_fcn()
+{
// read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
user_button_timer.stop();
- if (user_button_elapsed_time_ms > 200) {
- ToNextFunction += 1;}
- }
\ No newline at end of file
+ if (user_button_elapsed_time_ms > 200)
+ {
+ ToNextFunction += 1;
+ }
+}
\ No newline at end of file