Added some joint rotation to robotarm

Not sure if this is even right but I need to go to bed. So I'll look at
it more tomorrow