ZZAAG v3 mod

' zzaag UC01 firmware rev.A2

$regfile = "m32def.dat" ' Chip definition - atmega32
$crystal = 16000000 ' Crystal oscilator (16Mhz)
$baud = 19200 ' UART speed

Pwm_al Alias Ocr1al ' Right motor PWM output
Pwm_bl Alias Ocr1bl ' Left motor PWM output
Vreg Alias Portd.2 ' Enable H-driver circuit
Buzz Alias Portc.0 ' BuZzEr out
Cw_ccw_a Alias Portd.7 ' Direction control for right motor??
Cw_ccw_b Alias Portd.6 ' Direction control for left motor??

Config Porta = Input ' 0 - Acc Y, 1 - Acc X, 2 - Gyro X, 3 - Gyro Y, 6 - divided bat voltage
Config Portb = Input ' programming interface
Config Portc = Output ' some undefined GPIO to driver
Config Portd = Output ' PWM and other signals for driver, first to pins handle UART RxD/TxD
Config Adc = Single , Prescaler = 128 , Reference = Off ' Prepare ADC, probably for stering potentiometr
Start Adc ' Run ADC
Config Timer0 = Timer , Prescale = 1024 ' Config timer0 to be a cpu-time meter, it`s increase by 1 every 1024 cpu cycles
On Timer0 Tinter ' Run every 1024 cpu cycles
Portb = 3 ' Pull-up inputs

Dim Buzzer As Byte ' Buzzer status, will be assigned to real buzzer output (Buzz) in Tinter method
Dim Ad_adxl As Long ' ??
Dim Ad_gyro As Long ' ??
Dim Ad_batt As Integer ' ADC for battery?
Dim Ad_swi As Integer ' ??
Dim Total_adxl_gyro As Long ' ??
Dim Average_gyro As Long ' ??
Dim Average_batt As Long ' ??
Dim Drivea As Integer ' PWM for right motor
Dim Driveb As Integer ' PWM for left motor
Dim Buf1 As Long ' Temp?
Dim Buf As Long ' Temp?
Dim Buf2 As Long ' Temp?
Dim Buf5 As Integer ' Temp?
Dim Tilt_angle As Long ' Actual tilt angle of platform
Dim Drive_a As Integer ' ??
Dim Drive_b As Integer ' ??
Dim Drive_as As Integer ' ?? - never contain value diff from 0
Dim Drive_bs As Integer ' ?? - never contain value diff from 0
Dim Tcount As Integer ' Counter used to limit logs limit
Dim Drivespeed As Integer ' Your speed?

Dim Steeringsignal As Long ' ??
Dim Anglecorrection As Long ' Correction gyro using acc?
Dim Angle_rate As Long ' ??
Dim Balance_moment As Long ' ??
Dim Overspeed As Long ' ?? - never contain value diff from 0

Dim Mmode As Byte ' Operation mode, can be 1 (const _run), 0 (const _standby) or 2 (const _warmup)
Dim Overspeed_flag As Byte ' ?? -  never contain value diff from 0 (maybe its a part of unifinished warning system if you driving too fast?)
Dim Drive_sum As Long ' ??
Dim Ad_rocker As Integer ' ??

Dim Timeout As Long ' I think this is used in timer of two seconds before stoping engines when someone falloff the segway
Dim Delta As Byte ' ??
Dim Buf3 As Long ' Temp?
Dim Rockersq As Long ' Something with steering?

Dim Rocker_zero As Integer ' Contain "zero" position of steering pipe?
Dim Adxl_zero As Word ' Accelerometr zero point?
Dim Gyro_zero As Word ' Gyro zero poin?
Dim Loopct As Integer ' ??

Const First_adc_input = 0 ' ?? - never used in code
Const Last_adc_input = 6 ' ?? - never used in code
Const Adc_vref_type = $0x40 ' ?? - never used in code

Const Mdrivesumlimit = -43000 ' ??

Const Total_looptime = 500 ' ?? - Looptime For Filters
Const Total_looptime10 = 50 ' ?? - Looptime For Filters
Const Adxl_offset = -13 ' ?? - Stick Angle Offset
Const _run = 1
Const _standby = 0
Const _warmup = 2
Const Safespeed = 5000 ' safe speed /\ ?
Const Msafespeed = -5000 ' safe speed \/ ?
Const Sw_down = 50
Const Critical = 80

Const Battok = 1023 ' Battery is ok if voltage is 24V (adc counts to 1023 and resistors make divider to provide safe, 5V max output to CPU)
Const Batt_low = 990 ' Minimum input to ADC when battery is low

Const Max_pwm = 210 ' Maximum safe PWM value for /\
Const Mmax_pwm = -210 ' Maximum safe PWM value for \/

Declare Sub Get_bat_volt ' Read battery voltage from ADC
Declare Sub Get_tilt_angle ' Compute tilt angle using gyro and acc
Declare Sub Set_pwm ' Set PWM value for motors. It also cares about limitations
Declare Sub Algo ' ??
Declare Sub Process ' ??
Declare Sub Ini ' Initiation procedure
Declare Sub Getspeedlimit ' Read speed limit
Declare Sub Err_eeprom ' If eeprom error detected this will give a signal



' motorcontroller disabled / PWM-Signals to zero
Set Vreg
Pwm_al = 255 ' Right motor at 0 RPM
Pwm_bl = 0 ' Left motor at 0 RPM [its reversed]

Gosub Ini ' Some init stuff - it`s ASM

'********************* CALIBRATION *********************'
' First of all check state of jumper. It can be at calibration or operation position
If Pinb.0 = 0 Then ' Calibration mode
 Print "CLB-MDE"
 For Buf = 1 To 4 ' Four long beeps
  Set Buzz
  Waitms 75
  Reset Buzz
  Waitms 75
 Next Buf

 Waitms 500
 Buf1 = 0
 For Buf = 1 To 10 ' Get 10 samples from axis Y of gyro with 100ms resolution (so it will take 1 second)
  Buf3 = Getadc(3)
  Buf1 = Buf1 + Buf3
  Waitms 100
 Next Buf

 Buf5 = Buf1 / 10 ' Calculate middle value
 Delta = High(buf5) ' ??

 'store data in eeprom'
 Writeeeprom Delta , 2
 Writeeeprom Delta , 4
 Delta = Low(buf5) ' ??
 Writeeeprom Delta , 3
 Writeeeprom Delta , 5

 Buf1 = 0
 For Buf = 1 To 10 ' Get 10 samples from axis Y of gyro with 100ms resolution (so it will take 1 second)
  Buf3 = Getadc(1)
  Buf1 = Buf1 + Buf3
  Waitms 100
 Next Buf

 Buf5 = Buf1 / 10 ' Calculate middle value
 Delta = High(buf5) ' ??
 Writeeeprom Delta , 6
 Writeeeprom Delta , 8
 Delta = Low(buf5)
 Writeeeprom Delta , 7
 Writeeeprom Delta , 9
End If
'********************* END OF CALIBRATION *********************'


'********************* BOOTLOADER JUMPING *********************'
If Pinb.1 = 0 Then ' Bootloader jumper set
  Print "BTL-MDE"
  For Buf = 1 To 5 ' Five short beeps indicates bootloader mode
    Set Buzz
    Waitms 45
    Reset Buzz
    Waitms 75
  Next Buf
  jmp $3c00 ' Jump to bootloader code
End If
'********************* END OF BOOTLOADER JUMPING *********************'


'********************* RUNTIME CHECK&CONFIG *********************'
'read Calib.Data (pair 1)
Readeeprom Delta , 2
Buf = Delta * 256
Readeeprom Delta , 3
Buf = Buf + Delta

'read Calib.Data (pair 2)
Readeeprom Delta , 4
Buf3 = Delta * 256
Readeeprom Delta , 5
Buf3 = Buf3 + Delta

'set gyro_zero value'
Gyro_zero = Buf

'Datapairs not equal-> Epprom-err'
If Buf3 <> Buf Then
 Goto Err_eeprom
End If

'or empty eeporm (default value) ?
If Buf3 = 65535 Then
 Goto Err_eeprom
End If

'read Calib.Data (pair 1)
Readeeprom Delta , 6
Buf = Delta * 256
Readeeprom Delta , 7
Buf = Buf + Delta

'read Calib.Data (pair 2)
Readeeprom Delta , 8
Buf3 = Delta * 256
Readeeprom Delta , 9
Buf3 = Buf3 + Delta

'set adxl_zero value'
Adxl_zero = Buf

'Datapairs not equal-> Epprom-err'
If Buf3 <> Buf Then
 Goto Err_eeprom
End If

'or empty eeporm (default value) ?
If Buf3 = 65535 Then
 Goto Err_eeprom
End If

'set average_gyro
Average_gyro = Total_looptime * Gyro_zero
'********************* END OF RUNTIME CHECK&CONFIG *********************'



'********************* STEERING PIPE 0-POINT CALIBRATION *********************'
'Get Steering_zero Position'
Ad_rocker = 0
For Buf = 1 To 10 ' get 10 samples in 50ms - this eliminate errors
 Ad_rocker = Ad_rocker + Getadc(5)
 Waitms 5
Next Buf
Rocker_zero = Ad_rocker / 10 ' use middle calculateg from 10 samples
'********************* END OF STEERING PIPE 0-POINT CALIBRATION *********************'



'********************* NORMAL OPERATION *********************'
' Give 2 short beeps - system doesnt detected any runtime error, in future check here some security like RFID card
Set Buzz
Waitms 45
Reset Buzz
Waitms 75
Set Buzz
Waitms 45
Reset Buzz

Reset Vreg ' enable H-bridges circuit
Enable Interrupts ' Wake-up interrupts
Enable Timer0 ' ...and timer0

S1: ' Main loop - nothing to do, all work is done by the Timer0 interrupt
Goto S1 ' loop
'********************* END OF NORMAL OPERATION *********************'



'********************* GET BATTERY VOLTAGE *********************'
Sub Get_bat_volt
  Buf = Average_batt / Total_looptime
  Average_batt = Average_batt - Buf
  Average_batt = Average_batt + Ad_batt

  ' check voltage
  Buf = Batt_low * Total_looptime
  If Average_batt < Buf Then
   Set Buzz ' if too low turn turn buzzer on
  End If
End Sub
'********************* END OF GET BATTERY VOLTAGE *********************'


'********************* MOTORS PWM SET *********************'
Sub Set_pwm
 'Limiting PWM for right motor
 If Drive_a > Max_pwm Then
  Drive_a = Max_pwm
 End If

 If Drive_a < Mmax_pwm Then
  Drive_a = Mmax_pwm
 End If

 'set direction bit for right motor
 If Drive_a < 0 Then
  Drivea = Drive_a * -1
  Cw_ccw_a = 1
 End If

 If Drive_a >= 0 Then
  Drivea = Drive_a
  Cw_ccw_a = 0
 End If

 'Inverse signal to have 180° phaseshift to PWMB bcs motor is mounted 180*?
 Pwm_al = 255 - Drivea ' set right motor speed


 'Limiting PWM for left motor
  If Drive_b > Max_pwm Then
   Drive_b = Max_pwm
  End If
  If Drive_b < Mmax_pwm Then
   Drive_b = Mmax_pwm
  End If


 'set direction bit for left motor
 If Drive_b < 0 Then
  Driveb = Drive_b * -1
  Cw_ccw_b = 1
 End If

 If Drive_b >= 0 Then
  Driveb = Drive_b
  Cw_ccw_b = 0
 End If

 Pwm_bl = Driveb ' set left motor

 ' Idea - its a better to first calculate pwm`s and next set it eliminating delay between setting left and right motor
End Sub
'********************* END OF MOTORS PWM SET *********************'



'********************* MAIN ALGORITHM? *********************'
Sub Algo
 Buf = Tilt_angle + Anglecorrection
 Buf = Buf * 17
 Buf1 = Angle_rate * 20

 'calculate balance moment
 Balance_moment = Buf1 + Buf

 'check speedlimit
 Gosub Getspeedlimit

 ' calculate drive_sum
 ' damn ... but what is drive_sum?!
 Drive_sum = Drive_sum + Balance_moment

 ' limitting
 ' max speed limiter?
 If Drive_sum > 55000 Then
  Drive_sum = 55000
 End If
 If Drive_sum < -55000 Then
  Drive_sum = -55000
 End If

 'calculate drive speed
 Buf = Drive_sum / 125
 Buf1 = Balance_moment / 125
 Drivespeed = Buf + Buf1
End Sub
'********************* END OF MAIN ALGORITHM? *********************'


'********************* GET SPEED LIMIT *********************'
Sub Getspeedlimit
 If Drive_sum < 0 Then
  If Drive_sum < Mdrivesumlimit Then
   Anglecorrection = -13
   Buzzer = 1
   Set Buzz
  Else
   Anglecorrection = 0
  End If
 End If
End Sub
'********************* END OF GET SPEED LIMIT *********************'



'********************* MODE HANDLER *********************'
Sub Process
 ' First of all push some logs
 Tcount = Tcount + 1
 If Tcount > 30 Then //Send logs every 30 loops of whole program[?]
  Tcount = 1
  Print "ST-TA:" ; Tilt_angle ' Just angle of platform tilt
  Print "ST-ADXL:" ; Ad_adxl ' ??
  Print "ST-GYRO:" ; Ad_gyro ' ??
  Print "ST-AVBT:" ; Average_batt ' Some battery stuff
  Print "ST-ADBT" ; Ad_batt ' SOME battery stuff
  Print "ST-AVGR" ; Average_gyro ' ??
  Print "ST-MDE" ; Mmode ' Mode of operation (see constants at few first lines)
 End If


 'Steering calculation
 Rockersq = Rocker_zero - Ad_rocker
 If Rockersq > 0 Then
  ' ?? - Make it progressiv
  Rockersq = Rockersq * Rockersq
 End If

 If Rockersq < 0 Then
  Rockersq = Rockersq * Rockersq
  Rockersq = Rockersq * -1
 End If

 ' Hmm is this part decrase sensitivity of stearing when speed going up?
 Buf1 = Drive_sum / 600
 If Buf1 < 0 Then
  Buf1 = Buf1 * -1
 End If
 Buf1 = Buf1 + 77
 Rockersq = Rockersq / Buf1

 ' Some safety - limits the max. steering values
 If Rockersq > 25 Then Rockersq = 25
 If Rockersq < -25 Then Rockersq = -25


 ' Make the steering smooth
 If Steeringsignal < Rockersq Then
  Steeringsignal = Steeringsignal + 4
 End If

 If Steeringsignal > Rockersq Then
  Steeringsignal = Steeringsignal - 4
 End If

 ' Set speed and steering
 Drive_a = Drivespeed - Steeringsignal
 Drive_b = Drivespeed + Steeringsignal


 ' Need some seconds to compensate the gyro temp-drift
 If Mmode = _warmup Then
  Drive_a = 0
  Drive_b = 0
  Drive_as = 0
  Drive_bs = 0
  Drivespeed = 0
  Anglecorrection = 0
  Overspeed = 0
  Drive_sum = 0
  Total_adxl_gyro = 0
  Tilt_angle = 0

  If Loopct > 1100 Then
   Mmode = _standby

   ' Give some beeps - damn, but what it means?
   Set Buzz
   Waitms 20
   Reset Buzz
   Waitms 50
   Set Buzz
   Waitms 20
   Reset Buzz
  End If
 End If

 If Mmode = _standby Then
  Drive_a = 0
  Drive_b = 0
  Drive_as = 0
  Drive_bs = 0
  Drivespeed = 0
  Anglecorrection = 0
  Overspeed = 0

  Drive_sum = 0
  Total_adxl_gyro = 0
  Tilt_angle = 0


  Buf2 = Ad_adxl - Adxl_zero
  Buf2 = Buf2 + Adxl_offset
  Buzzer = 0
  Timeout = 0

  ' If someone stand on platform
  If Ad_swi > Sw_down Then
   Mmode = _run ' …change mode to RUN
  End If
 End If

 If Mmode = _run Then
 'check platform buttons

  If Ad_swi > Sw_down Then ' person is on platform
   If Buzzer = 1 Then 'if buzzer still sounds turn it off
    Buzzer = 0
   End If
  End If

  If Ad_swi < Sw_down Then ' no person on board
   If Drive_sum < 0 Then ' for moving backwards?
    If Drive_sum < Msafespeed Then ' hmm this proably turn on buzzer only if platform is moving
     Buzzer = 1
    End If

    If Drive_sum > Msafespeed Then ' similar to above but hmm …
     Buzzer = 0
    End If
   End If

   If Drive_sum > 0 Then ' form moving normal?
    If Drive_sum > Safespeed Then
     Buzzer = 1
    End If

    If Drive_sum < Safespeed Then
     Buzzer = 0
    End If
   End If

   If Buzzer = 1 Then
    If Timeout > Critical Then ' if buzzer is on and timeout (2 seconds?) is reached turn off engines
     Mmode = _standby
    End If
   End If

  End If ' end "no person on board" if

  If Buzzer = 0 Then ' if buzzer turned off…
   Timeout = 0 ' ...reset critical timeout
  End If

  Timeout = Timeout + Buzzer ' some freaky trick (?)
 End If ' end of "if Mmode = _run" if
End Sub
'********************* END OF MODE HANDLER *********************'

'********************* INIT STUFF *********************'
Sub Ini
  $asm
  ldi r16,0
  Out Porta , R16
  Out Ddra , R16

  'ldi r16,$e2
  'Out Portb , R16
  'ldi r16,$e3
  'Out Ddrb , R16

  ldi r16,0
  Out Portc , R16
  ldi r16,255
  Out Ddrc , R16

  ldi r16,$34
  Out Portd , R16
  ldi r16,$ff
  Out Ddrd , R16

  ldi r16,$b1                                               'b1=8bit ,b2=9bit,b3=10bit
  Out Tccr1a , R16
  ldi r16,$01                                               ' 01 no prescaler!
  Out Tccr1b , R16
  ldi r16,0
  Out Tcnt1h , R16
  Out Tcnt1l , R16
  Out Icr1h , R16
  Out Icr1l , R16
  Out Ocr1ah , R16
  ldi r16,$ff
  'Out Ocr1ah , R16
  Out Ocr1al , R16
  ldi r16,0
  'Out Ocr1bh , R16
  ldi r16,0
  Out Ocr1bl , R16

  Out Assr , R16
  Out Tccr2 , R16
  Out Tcnt2 , R16
  Out Ocr2 , R16
  $end Asm

 Average_batt = Total_looptime * Battok
 Total_adxl_gyro = 0
 Average_gyro = 0
 Drivea = 0
 Driveb = 0
 Tilt_angle = 0
 Drive_a = 0
 Drive_b = 0
 Drivespeed = 0
 Steeringsignal = 0
 Anglecorrection = 0
 Angle_rate = 0
 Balance_moment = 0
 Overspeed = 0

 Mmode = _warmup
 Overspeed_flag = 0
 Drive_sum = 0
 Ad_rocker = 0
 Tcount = 0
 Steeringsignal = 0
 Drive_as = 0
 Drive_bs = 0

 Timeout = 0
 Delta = 0
 Loopct = 0
End Sub
'********************* END OF INIT STUFF *********************'

'********************* TIMER0 INT. HANDLER *********************'
' Handle interputs from Timer0
Tinter:
 Disable Interrupts ' prevents from re-entering "Tinter" if this block of code exceed 1024 cpu cycles which cause infinite-loop ?
 Gosub Get_bat_volt ' Masure battery voltage
 Gosub Get_tilt_angle ' Calculate platform tilt angle

 Gosub Algo
 Gosub Process
 Gosub Set_pwm ' Set PWM`s to motors
 Buzz = Buzzer ' If some code above turn on buzzer turn it real (Buzzer is int var, Buzz is link to output). I don`t know why author of this code used extra variable for that insted of setting Buzz directly

 Timer0 = 100 ' why 100?

 Enable Interrupts
Return


'********************* END OF TIMER0 INT. HANDLER *********************'

'********************* EEPROM-ERROR *********************'
Sub Err_eeprom
 Print "ERR-EEPROM" ' EEPROM error
  S3:
   For Buf = 1 To 6 ' Six long beeps repeated every 2 seconds
    Set Buzz
    Waitms 75
    Reset Buzz
    Waitms 75
  Next Buf

  Wait 2
  Goto S3 ' Why its not a classic DO-LOOP?
End Sub
'********************* END OF EEPROM-ERROR *********************'

'********************* GET TILT ANGLE *********************'
 Sub Get_tilt_angle
  Ad_gyro = Getadc(3)
  Ad_adxl = Getadc(1)
  Ad_batt = Getadc(6)
  Ad_rocker = Getadc(5)
  Ad_swi = 1024 - Getadc(4)

  Buf = Total_adxl_gyro / Total_looptime
  Total_adxl_gyro = Total_adxl_gyro - Buf

  ' ADXL part
  Buf = Ad_adxl - Adxl_zero
  Buf = Buf + Adxl_offset

  Total_adxl_gyro = Total_adxl_gyro + Buf

  ' Gyro part
  Buf1 = Average_gyro / Total_looptime
  Average_gyro = Average_gyro - Buf1

  Average_gyro = Average_gyro + Ad_gyro
  Buf1 = Average_gyro / Total_looptime10

  ' calculate the Angle Rate
  Buf = Ad_gyro * 10
  Buf1 = Buf1 - Buf
  Buf1 = Buf1 * 35
  Buf1 = Buf1 / 100
  Angle_rate = Buf1

  ' calculate the Tilt Angle
  Total_adxl_gyro = Total_adxl_gyro + Angle_rate
  Tilt_angle = Total_adxl_gyro / Total_looptime10


  Loopct = Loopct + 1

  Return
 End Sub
'********************* END OF GET TITL ANGLE *********************'

End ' empty infinite loop :)