Poking around it appears likely that you are referring to "Dynamics of Structures: Theory and Application to Earthquake Engineering" by Anil K. Chopra, chapter 5, 5.1 Time-Stepping Methods . However you have not indicated which edition, with 4 known numbered editions, and with one named "International Edition". Or perhaps you are referring to "Dynamics of Structures: A Primer" by Anil K. Chopra, or to "Earthquake Dynamics of Structures: A Primer" by Anil K. Chopra and Barbara Zeiders.
The first of those has the most supporting evidence, but the edition matters because there might have been an error that got corrected in later versions.
Or if they do not give code in the book, then the error is in your introduction of p without defining it.
Have you considered using the implementation in the File Exchange ?