Pi controller for all motors

function [ success ] = example_pi_controller

kp = 3;

ki = 0.05;

ea09com_init;

ea09com_bind( 8809 );

ea09com_setAddress( '172.26.254.254', 8809 );

send_RFV;

while( 0 == [num_FV] )

   ea09com_update;

end;

takeFirst_FV

for motor = 0:2

%set monitoring of motor 0,1,2 on

send_SetMMONITOR( motor, 1 );

send_SetMCTRL( motor, 2 );

end;

tstart = clock;

i = zeros(1,3);

u = zeros(1,3);

numReceived = zeros(1,3);

%run for 10s

while( etime(clock,tstart) < 2 )

  while( [num_MessageA(0)]-numReceived(1) == 0 || [num_MessageA(1)]-numReceived(2) == 0 || [num_MessageA(2)]-numReceived(2) == 0 )

       ea09com_update

  end;

   numReceived(1) = num_MessageA(0);

   numReceived(2) = num_MessageA(1);

   numReceived(3) = num_MessageA(2);

  for motor = 1:3

       messageA = last_MessageA( motor-1 );

       speed_setpoint = messageA(3);

       actual_speed = messageA(4);

       e = speed_setpoint - actual_speed;

       i(motor) = i(motor) + e;

       u(motor) = kp * ( e + ki * i(motor) );

      if( u(motor) > 255 )

           u(motor) = 255;

      elseif( u(motor) < -255 )

           u(motor) = -255;

      end;

  end;

   u

   send_SetControlPoint( u(1), u(2), u(3), 0 )

end;

for motor = 0:2

%set monitoring of motor 0,1,2 on

send_SetMMONITOR( motor, 0 );

send_SetMCTRL( motor, 1 );

end;

index = zeros(1,3) + 1;

startTime = 0;

num_MessageA( 0 );

messageA = takeFirst_MessageA(0);

startTime = messageA(2);

while( [num_MessageA(0)] > 0 && [num_MessageA(1)] > 0 && [num_MessageA(2)] > 0 )

  for motor = 1:3

       messageA = takeFirst_MessageA(motor-1);

       mytime(motor,index(motor)) = messageA(2) - startTime;

       speed_setpoint(motor,index(motor)) = messageA(3);

       actual_speed(motor,index(motor)) = messageA(4);

       current(motor,index(motor)) = messageA(5);

       control_point(motor,index(motor)) = messageA(6);

       index(motor) = index(motor) + 1;

  end;

end;

ea09com_close;

for motor = 1:3

   subplot(3,1,motor);

   hold;

   title(['Motor ', num2str(motor-1), ' readings']);

   plot( mytime(motor,:), speed_setpoint(motor,:) / 255, 'Color', [1 0 0] );

   plot( mytime(motor,:), actual_speed(motor,:) / 255, 'Color', [0 1 0]  );

   plot( mytime(motor,:), current(motor,:) / 4096, 'Color', [0.3 0.3 0.3 ]  );

   plot( mytime(motor,:), control_point(motor,:) / 255, 'Color', [0 0 1]  );

   legend( 'speed set-point', 'actual speed', 'current', 'control point' );

   hold;

end;