Revision f4a85021 paintboard/code/motor.c

View differences:

paintboard/code/motor.c
19 19
int8_t motor1_speed;
20 20
int8_t motor2_speed;
21 21

  
22
inline uint8_t abs_clamp(int8_t n)
23
{
24
  if (n == -128)
25
    return 127;
26
  else if (n < 0)
27
    return -n;
28
  else
29
    return n;
30
}
31

  
22 32
void motor_init(void)
23 33
{
24 34

  
......
44 54
/* Speed must be between -127 and 127 */
45 55
{
46 56
  motor1_speed = speed;
47
  OCR2A = (uint8_t)abs(speed)*2;
57
  OCR2A = abs_clamp(speed)*2;
48 58
  if(speed>0) //go forwards
49 59
  {
50 60
    PORTC |= (1<<PC0);
61
    PORTC &= ~(1<<PC1);
51 62
    PORTC = PORTC & ~(1<<PC1);
52 63
  }  
53 64
  if(speed<0) //go backwards
54 65
  {
66
    PORTC &= ~(1<<PC0);
55 67
    PORTC |= (1<<PC1);
56 68
    PORTC = PORTC & ~(1<<PC0);
57 69
  }
......
66 78
/* Speed must be between -127 and 127 */
67 79
{
68 80
  motor2_speed = speed;
69
  OCR2B = (uint8_t)abs(speed)*2;
81
  OCR2B = abs_clamp(speed)*2;
70 82
  if(speed>0) //go forwards
71 83
  {
72 84
    PORTC |= (1<<PC2);
85
    PORTC &= ~(1<<PC3);
73 86
    PORTC = PORTC & ~(1<<PC3);
74 87
  }  
75 88
  if(speed<0) //go backwards
76 89
  {
90
    PORTC &= ~(1<<PC2);
77 91
    PORTC |= (1<<PC3);
78 92
    PORTC = PORTC & ~(1<<PC2);
79 93
  }

Also available in: Unified diff