' This is a program translated from pBasic for BasicStamp2. The original is work
' of John Fisher, January 2007, published on the Parallax forum:
' http://forums.parallax.com/forums/default.aspx?f=21&p=1&m=162898
'
' The robot uses a Ping))) distance sensor to measure the distance to the ground.
' A Sharp GP2D12 may be used, but is not as reliable, it has spikes from time to
' time and the robot loses balance, also the analog conversion takes a lot of time.
'


$regfile = "m168def.dat"
$crystal = 8000000
$baud = 19200


'Aliases
Led_status Alias Portb.5                                    ' I use a jumper to switch
Dif Alias Portb.5                                           ' between Led and Dif
Config Portb.5 = Output

Ping_port Alias Portd.7                                     ' Ping sensor pin
Ping_pin Alias Pind.7

'Variables
Dim Distance As Single                                      ' distance variable
Dim Ptime As Integer                                        ' raw value from sensors
Dim Drive As Integer                                        ' PWM value sent to Servos
Dim Error(5) As Integer                                     ' array of 5 Error elements
Dim P As Integer                                            ' proportional term
Dim I As Integer                                            ' integral term
Dim D As Integer                                            ' derivative term
Dim Setpoint As Integer                                     ' perfect balance point value
Dim Count As Byte                                           ' counter
Dim Period As Word                                          ' period for generating sounds
Dim Lwheel As Integer                                       ' left wheel variable
Dim Rwheel As Integer                                       ' right wheel variable

'Constants
Const T_per_cm = 29.034                                     ' 1cm = 29.034us

'tests should be made to determine acurate Min, Mid and Max values for the servos
Const Smin = 2000
Const Midl = 2990                                           ' center for servos, they should be stoped
Const Midr = 3005
Const Smax = 4000

'ADC Channels are just numbers passed to GetADC() function
Const Pot = 5

'Ping PID constants
Const Kp = 2                                                ' #
Const Ki = 3                                                ' #
Const Kd = 4                                                ' #

'Meaningful names for error array index:
Const Current = 1
Const Sum = 2
Const Last = 3
Const Secondtolast = 4
Const Delta = 5

'ADC setup
Config Adc = Single , Prescaler = Auto
Start Adc

'Timer setup
Config Timer1 = Timer , Prescale = 8
Stop Timer1

'Initialisation
 Wait 1
  Gosub Program_start_sound
 Wait 1

'================================================
  Setpoint = 328                                            ' # Ping setpoint

'this must be equal with the Ping flight time (Ptime) when the robot is perfectly balanced
'use a Pot to determine the perfect value, or use buttons to increase/decrease it
'================================================


'servo setup
Config Portd.4 = Output                                     ' servo pins as outputs
Config Portd.5 = Output
Portd.4 = 0                                                 'set the pin low
Portd.5 = 0

Servo Alias Portd
Sleft Alias 4
Sright Alias 5

Start Timer1                                                'timer1 on

'----------------------------------------------------------------------------
'Main program
'----------------------------------------------------------------------------

Main_prog:

Do
   Gosub Read_ping_sensor
   Gosub Pid
   Gosub Motors
   Waitms 12
Loop
'----------------------------------------------------------------------------
'Subroutines
'----------------------------------------------------------------------------
Read_ping_sensor:
   Config Ping_port = Output                                'set pin as output for ping
   Ping_port = 1                                            'set high
   Timer1 = 0                                               '5us  delay
   Do
   Loop Until Timer1 >= 5
   Ping_port = 0                                            'set low
   Config Ping_port = Input                                 'set pin as input for echo
   Ping_port = 1                                            'turn on pull up
   Bitwait Ping_pin , Set                                   'wait until a high respon from ping (hold off time)
   Timer1 = 0                                               'reset timer1 value, ready to count time elapsed
   Bitwait Ping_pin , Reset                                 'wait until a low response from ping (distance time)
   Ptime = Timer1                                           'copy timer1 value for distance measurement
     'Print "Ping = " ; Ptime
   'Distance = Ptime / 2                                     ' we need only one way sound trip
   'Distance = Distance / T_per_cm                           ' distance in cm
     'Print "Ping distance = " ; Distance
Return

Pid:
  Error(current) = Setpoint - Ptime                         ' Ping values proportional with distance
  P = Error(current) * Kp
  Error(sum) = Error(current) + Error(last)
  Error(sum) = Error(sum) + Error(secondtolast)
  I = Ki * Error(sum)
  Error(delta) = Error(current) - Error(last)
  D = Kd * Error(delta)
  Drive = P + D
  Drive = Drive + I
  Error(secondtolast) = Error(last)
  Error(last) = Error(current)
     'Print "PID value = " ; Drive
Return

Motors:
  Lwheel = Midl + Drive
  Rwheel = Midr - Drive
  Pulseout Servo , Sleft , Lwheel
  Pulseout Servo , Sright , Rwheel
     'Print "Left = " ; Lwheel
     'Print "Right = " ; Rwheel
Return

Program_start_sound:
  For Count = 1 To 4
    'Sound Speaker , Duration , Period
    Period = Count * 50
    Period = 1300 - Period
    Sound Dif , 80 , Period
    Waitms 100
  Next
Return