test
Fork of motor by
Embed:
(wiki syntax)
Show/hide line numbers
motor.cpp
00001 #include "motor.h" 00002 00003 int motor::write(int spd) 00004 { 00005 if(!inited) return -1; 00006 char cmd[1]; 00007 if (spd > 0) { 00008 if(spd >= 255) spd = 255; 00009 spd = map(spd, 1, 255, cwMin + 1, cwMax); 00010 cmd[0] = spd / 2 + 0b10000000; 00011 direction = 1; 00012 } else if (spd < 0) { 00013 if(spd <= -255) spd = -255; 00014 spd = map(spd, -1, -255, ccwMin + 1, ccwMax); 00015 cmd[0] = spd / 2; 00016 direction = 0; 00017 } else { 00018 cmd[0] = H_B << 7; 00019 } 00020 speed = cmd[0] << 1; 00021 return i2c->write(number, cmd, 1); 00022 } 00023 00024 uint8_t motor::make(int spd) 00025 { 00026 char cmd[1]; 00027 if (spd > 0) { 00028 if(spd >= 255) spd = 255; 00029 spd = map(spd, 1, 255, cwMin + 1, cwMax); 00030 cmd[0] = spd / 2 + 0b10000000; 00031 } else if (spd < 0) { 00032 if(spd <= -255) spd = -255; 00033 spd = map(spd, -1, -255, ccwMin + 1, ccwMax); 00034 cmd[0] = spd / 2; 00035 } else { 00036 cmd[0] = 0; 00037 } 00038 return cmd[0]; 00039 } 00040 00041 uint8_t motor::init(uint8_t t, I2C *i) 00042 { 00043 char cmd[2] = {t>>1, t>>1}; 00044 char data[1]; 00045 00046 inited = true; 00047 00048 number = t; 00049 i2c = i; 00050 speed = 0; 00051 direction = 1; 00052 cwMax = 255; 00053 ccwMax = 255; 00054 cwMin = 0; 00055 ccwMin = 0; 00056 00057 while(1) { 00058 i2c->write(t, cmd, 2); 00059 i2c->read(t | 1, data, 1); 00060 if(data[0] == (t >> 1)) break; 00061 } 00062 write(0); 00063 return 0; 00064 } 00065 00066 long motor::map(long x, long in_min, long in_max, long out_min, long out_max) 00067 { 00068 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 00069 }
Generated on Mon Aug 8 2022 00:05:40 by
1.7.2
