Adding stretchercise to web interface directory

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()