From c250acd148eff6aacaff254203129cc80a24b072 Mon Sep 17 00:00:00 2001 From: algarv Date: Fri, 16 Sep 2022 14:54:06 -0700 Subject: [PATCH] Adding stretchercise to web interface directory --- stretchercise/fonts | 1 + stretchercise/stretch_robot_exercise.py | 355 ++++++++++++++++++++++++ 2 files changed, 356 insertions(+) create mode 160000 stretchercise/fonts create mode 100755 stretchercise/stretch_robot_exercise.py diff --git a/stretchercise/fonts b/stretchercise/fonts new file mode 160000 index 0000000..96da5c8 --- /dev/null +++ b/stretchercise/fonts @@ -0,0 +1 @@ +Subproject commit 96da5c8b6cdc1b91d2ee58efef3f0402f5a47217 diff --git a/stretchercise/stretch_robot_exercise.py b/stretchercise/stretch_robot_exercise.py new file mode 100755 index 0000000..f652053 --- /dev/null +++ b/stretchercise/stretch_robot_exercise.py @@ -0,0 +1,355 @@ +#!/usr/bin/python + +from __future__ import print_function + +import stretch_body.robot as rb +import time +import stretch_body.hello_utils as hu +hu.print_stretch_re_use() +import pygame +from pygame.locals import * + +pygame.init() + +font_file_path = 'fonts/OpenSans/' + +bg_color = (19,42,57) +hello_pink = (237,194,209) +screen = pygame.display.set_mode([1920,1080]) +blank_background = pygame.Rect(0, 0, 1920, 1080) + +button_width = 300 +button_height = 100 +spacer = button_height + 25 + +left_column = (1920 // 2) - 400 - (button_width // 2) +right_column = (1920 // 2) + 400 + (button_width // 2) + +button_font = pygame.font.Font(font_file_path + 'OpenSans-Regular.ttf', 50) +title_font = pygame.font.Font(font_file_path + 'OpenSans-Bold.ttf', 75) +counter_font = pygame.font.Font(font_file_path + 'OpenSans-ExtraBold.ttf', 750) +text_font = pygame.font.Font(font_file_path + 'OpenSans-Bold.ttf', 300) + +class Stretchercises: + def __init__(self): + self.robot = rb.Robot() + self.robot.startup() + + def crunches(self, target_count): + + self.robot.end_of_arm.move_to('wrist_yaw', 3.1415) + self.robot.arm.move_to(0.45) + self.robot.lift.move_to(0.30) + self.robot.push_command() + + count_down(3) + + count = target_count + while count > 0: + self.robot.arm.pull_status() + flag = False + + while self.robot.arm.status['force'] > 15.0: + if not flag: + count -= 1 + print_count(count) + + flag = True + + self.robot.pimu.trigger_beep() + #self.robot.arm.move_to(0.45) + self.robot.push_command() + + def wall_sits(self, run_time): + self.robot.end_of_arm.move_to('wrist_yaw', 0.0) + self.robot.arm.move_to(0.50) + self.robot.lift.move_to(0.75) + self.robot.push_command() + time.sleep(10) + count_down(3) + while True: + self.robot.lift.move_by(-0.05, contact_thresh_pos_N=10.0) + self.robot.push_command() + self.robot.lift.pull_status() + if self.robot.lift.motor.status['in_guarded_event']: + break + + count_down(run_time) + self.robot.lift.move_to(0.75) + self.robot.push_command() + + def touches_sequence(self, run_time, speed='medium', upper=10, lower=1): + if speed == 'slow': + pause = 10 + elif speed == 'medium': + pause = 5 + elif speed == 'fast': + pause = 1 + + upper /= 10.0 + lower /= 10.0 + + self.robot.end_of_arm.move_to('wrist_yaw', 3.1415) + self.robot.arm.move_to(0.0) + self.robot.lift.move_to(lower) + self.robot.push_command() + time.sleep(10) + + start_time = time.time() + + while time.time() < start_time + run_time: + + self.robot.arm.move_to(0.5) + self.robot.push_command() + time.sleep(pause) + + self.robot.arm.move_to(0.0) + self.robot.lift.move_to(upper) + self.robot.push_command() + time.sleep(pause) + + self.robot.arm.move_to(0.5) + self.robot.push_command() + time.sleep(pause) + + self.robot.arm.move_to(0.0) + self.robot.lift.move_to(lower) + self.robot.push_command() + time.sleep(pause) + + + +''' +Modified rom baraltech +https://github.com/baraltech/Menu-System-PyGame +''' + +class Button(): + def __init__(self, pos, text_input, text_color, base_color, hovering_color, press_callback=None): + self.x_pos = pos[0] + self.y_pos = pos[1] + self.font = button_font + self.base_color, self.hovering_color = base_color, hovering_color + if press_callback is not None: + self.callback = press_callback + self.text_input = text_input + self.text_color = text_color + self.text = self.font.render(self.text_input, True, self.text_color) + self.text_rect = self.text.get_rect() + self.text_rect.center = (self.x_pos + button_width//2, self.y_pos + button_height//2) + self.button_rect = pygame.Rect(self.x_pos, self.y_pos, button_width, button_height) + pygame.draw.rect(screen, hello_pink, self.button_rect, 0) + + def update(self): + pygame.draw.rect(screen, hello_pink, self.button_rect, 0) + screen.blit(self.text, self.text_rect) + + def checkForInput(self, mouse_x, mouse_y): + if self.button_rect.collidepoint(mouse_x, mouse_y): + return True + return False + + def changeColor(self): + pygame.draw.rect(screen, self.hovering_color, pygame.Rect(self.x_pos, self.y_pos, button_width, button_height), 0) + screen.blit(self.text, self.text_rect) + + def pressed(self): + self.callback() + +def count_down(time): + screen.fill(bg_color) + pygame.display.update() + + count = time + pygame.time.set_timer(pygame.USEREVENT, 1000) + text = counter_font.render(str(count), True, hello_pink, bg_color) + textRect = text.get_rect() + textRect.center = (1920 // 2, 1080//2) + screen.blit(text, textRect) + pygame.display.update() + while count > 0: + screen.fill(bg_color) + + for e in pygame.event.get(): + if e.type == pygame.USEREVENT: + count -= 1 + text = counter_font.render(str(count), True, hello_pink, bg_color) + textRect = text.get_rect() + textRect.center = (1920 // 2, 1080//2) + screen.blit(text, textRect) + + pygame.display.update() + + text = counter_font.render('Go!', True, hello_pink, bg_color) + textRect = text.get_rect() + textRect.center = (1920//2, 1080//2) + screen.blit(text, textRect) + pygame.display.update() + +def print_count(num): + screen.fill(bg_color) + pygame.display.update() + text = counter_font.render(str(num), True, hello_pink, bg_color) + textRect = text.get_rect() + textRect.center = (1920 // 2, 1080//2) + screen.blit(text, textRect) + + pygame.display.update() + +def main_menu(): + screen.fill(bg_color) + mouse_x, mouse_y = 0, 0 + + text = title_font.render('S T R E T C H E R C I S E', True, hello_pink, bg_color) + textRect = text.get_rect() + textRect.center = (1920 // 2, 100) + screen.blit(text, textRect) + pygame.display.update() + + crunches = Button([left_column, 250], "Crunches", bg_color, hello_pink, (214, 199, 204), crunches_callback) + wall_sits = Button([left_column, 250 + spacer], "Wall Sits", bg_color, hello_pink, (214, 199, 204), wall_sits_callback) + touches = Button([left_column, 250 + 2*spacer], "Touches", bg_color, hello_pink, (214, 199, 204), touches_callback) + + button_list = [crunches, wall_sits, touches] + + for button in button_list: + button.update() + + while True: + + for event in pygame.event.get(): + if event.type == QUIT or (event.type == KEYUP and event.key == K_ESCAPE): + pygame.quit() + elif event.type == MOUSEMOTION: + mouse_x, mouse_y = event.pos + elif event.type == MOUSEBUTTONDOWN: + for button in button_list: + if button.checkForInput(mouse_x, mouse_y): + button.pressed() + screen.fill(bg_color) + text = title_font.render('S T R E T C H E R C I S E', True, hello_pink, bg_color) + textRect = text.get_rect() + textRect.center = (1920 // 2, 100) + screen.blit(text, textRect) + pygame.display.update() + + crunches = Button([left_column, 250], "Crunches", bg_color, hello_pink, (214, 199, 204), crunches_callback) + wall_sits = Button([left_column, 250 + spacer], "Wall Sits", bg_color, hello_pink, (214, 199, 204), wall_sits_callback) + touches = Button([left_column, 250 + 2*spacer], "Touches", bg_color, hello_pink, (214, 199, 204), touches_callback) + + for button in button_list: + button.update() + if button.checkForInput(mouse_x, mouse_y): + button.changeColor() + + + pygame.display.update() + +def crunches_callback(): + screen.fill(bg_color) + pygame.display.update() + stretchercises.crunches(15) + + screen.fill(bg_color) + pygame.display.update() + + text = text_font.render("Nice work!", True, hello_pink, bg_color) + textRect = text.get_rect() + textRect.center = (1920 // 2, 1080//2) + screen.blit(text, textRect) + pygame.display.update() + + time.sleep(5) + +def wall_sits_callback(): + screen.fill(bg_color) + pygame.display.update() + text = text_font.render("Get Ready", True, hello_pink, bg_color) + textRect = text.get_rect() + textRect.center = (1920 // 2, 1080//2) + screen.blit(text, textRect) + pygame.display.update() + stretchercises.wall_sits(30) + + + screen.fill(bg_color) + text = text_font.render("Nice work!", True, hello_pink, bg_color) + textRect = text.get_rect() + textRect.center = (1920 // 2, 1080//2) + screen.blit(text, textRect) + pygame.display.update() + + time.sleep(5) + +def slow_button(): + return 'slow' + +def medium_button(): + return 'medium' + +def fast_button(): + return 'fast' + +def touches_callback(): + screen.fill(bg_color) + pygame.display.update() + + mouse_x, mouse_y = 0, 0 + + text = title_font.render('Cross-Body Touch Stretch', True, hello_pink, bg_color) + textRect = text.get_rect() + textRect.center = (1920 // 2, 100) + screen.blit(text, textRect) + + slow = Button([1920//2 - button_width*1.5 - 25, 250], "slow", bg_color, hello_pink, (214, 199, 204), slow_button) + medium = Button([1920//2 - button_width*0.5, 250], "medium", bg_color, hello_pink, (214, 199, 204), medium_button) + fast = Button([1920//2 + button_width*0.5 + 25, 250], "fast", bg_color, hello_pink, (214, 199, 204), fast_button) + + speed_buttons = [slow, medium, fast] + + go = Button([1920//2 + button_width*0.5, 1080//2], "START", bg_color, hello_pink, (214, 199, 204)) + + pygame.display.update() + while True: + + for event in pygame.event.get(): + if event.type == QUIT or (event.type == KEYUP and event.key == K_ESCAPE): + pygame.quit() + elif event.type == MOUSEMOTION: + mouse_x, mouse_y = event.pos + elif event.type == MOUSEBUTTONDOWN: + speed = 'medium' + for button in speed_buttons: + if button.checkForInput(mouse_x, mouse_y): + speed = button.pressed() + + if go.checkForInput(mouse_x, mouse_y): + count_down(3) + stretchercises.touches_sequence(60, speed = speed) + return True + + slow = Button([1920//2 - button_width*1.5 - 25, 250], "slow", bg_color, hello_pink, (214, 199, 204), slow_button) + medium = Button([1920//2 - button_width*0.5, 250], "medium", bg_color, hello_pink, (214, 199, 204), medium_button) + fast = Button([1920//2 + button_width*0.5 + 25, 250], "fast", bg_color, hello_pink, (214, 199, 204), fast_button) + + go = Button([1920//2 + button_width*0.5, 1080//2], "START", bg_color, hello_pink, (214, 199, 204)) + + for button in [slow, medium, fast, go]: + button.update() + if button.checkForInput(mouse_x, mouse_y): + button.changeColor() + + pygame.display.update() + + screen.fill(bg_color) + text = text_font.render("Nice work!", True, hello_pink, bg_color) + textRect = text.get_rect() + textRect.center = (1920 // 2, 1080//2) + screen.blit(text, textRect) + pygame.display.update() + + time.sleep(5) + +stretchercises = Stretchercises() +main_menu() +pygame.quit() \ No newline at end of file