ss555/cart_pole

Inverted pendulum with deep reinforcement learning and model-based control methods

cart_pole's Issues

INTEFACE

'#!/usr/bin/env python3
#wifi latency aller-retour can go up to 0.1s, alors que par fil: moins de 1ms
#%%
import time
import os
import serial
import pandas as pd
pd.options.plotting.backend = "plotly"
import timeit
import numpy as np
import sys
import time
import random
import pigpio

import board
import busio
import adafruit_ads1x15.ads1115 as ADS
from adafruit_ads1x15.ads1x15 import Mode
from adafruit_ads1x15.analog_in import AnalogIn
from datetime import datetime
import socket
HOST = '192.168.19.125'#'c0:3e:ba:6c:9e:eb'#'10.42.0.1'#wifi-hotspot # The server's hostname or IP address
PORT = 65432 # The port used by the server
import math
import numpy as np
#rpi
import time
from matplotlib import pyplot as plt
import logging
from logging import info, basicConfig
import pigpio
import sys
FORMAT='utf-8'
PI_INPUT = pigpio.INPUT
PI_PUD_UP = pigpio.PUD_UP
pi = pigpio.pi()

if not pi.connected:
exit()
print('exit')

logname='TCP_DEBUG'
basicConfig(filename=logname,
filemode='w',#'a' for append
format='%(asctime)s,%(msecs)d %(name)s %(levelname)s %(message)s',
datefmt='%H:%M:%S',
level=logging.DEBUG)
start_time = time.time()

if name == "main":
start = timeit.default_timer()
data_list = []
TIMESTEP = 0.003 # in s
DURATION = 60 # in s
STEPS = 800

RATE = 860
# Create the I2C bus
# i2c = busio.I2C(board.SCL, board.SDA)
i2c = busio.I2C(board.SCL, board.SDA, frequency=1000000)

# Create the ADC object using the I2C bus
ads = ADS.ADS1115(i2c)

# ADC Configuration
ads.mode = Mode.CONTINUOUS
ads.data_rate = RATE

# Create single-ended input on channel 0
adc2 = AnalogIn(ads, ADS.P2) ## force 1
adc3 = AnalogIn(ads, ADS.P3) ## force 2
# First ADC channel read in continuous mode configures device
# and waits 2 conversion cycles
_ = adc2.value
_ = adc3.value
MIN_ANGLE = 30
MAX_ANGLE = 150
MIN_WIDTH = 800 ## µs
MAX_WIDTH = 2200 ## µs
MID_WIDTH = (MIN_WIDTH+MAX_WIDTH)/2

T = (1/freq)*1000 # in ms

t0 = get_current_milli_time() 
for j in range(STEPS+1):

    duration = timeit.default_timer()-start
    force1 = adc2.voltage
    force2 = adc3.voltage
    forcex = force1 - offsetF1
    forcey = force2 - offsetF2
    try:
        #### to verify the proprio angle, we test the harmonic one
        currentTime = get_current_milli_time()
        # angle = alpha0*widthScale*np.sin(omega*(currentTime-t0)/1000)/(MAX_WIDTH-MIN_WIDTH)/3.14*180
        # width = alpha0*widthScale*np.sin(omega*(currentTime-t0)/1000)+MID_WIDTH
        moment = currentTime-t0
        angle = alpha0*np.piecewise(moment, [moment%T<=(T/2), moment%T>(T/2)], [-1, 1])
        width = MID_WIDTH+widthScale*angle
        ###################################
        #print('angle', angle)
        #print('width', width)
        pi.set_servo_pulsewidth(pin, width)
        # time.sleep(0.05)
    except:
        pass

    data_list.append({'time':duration, 'Fx':forcex, 'Fy':forcey, 'phi_c':angle, 'pwm':width})
    time.sleep(TIMESTEP)

with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
    info('connecting')
    s.connect((HOST, PORT))

    pi.set_servo_pulsewidth(pin, width)
    pi.stop()'